├── tests ├── __init__.py └── test_pipeline.py ├── autompc ├── control │ ├── .gitignore │ ├── __init__.py │ ├── zero.py │ ├── controller.py │ └── lqr.py ├── sysid │ ├── .gitignore │ ├── __init__.py │ ├── dummy_nonlinear.py │ ├── linearize.py │ ├── dummy_linear.py │ ├── rnn.py │ ├── basis_funcs.py │ ├── stable_koopman.py │ ├── arx.py │ ├── koopman.py │ └── model.py ├── tasks │ ├── __init__.py │ └── task.py ├── utils │ ├── __init__.py │ ├── make_utils.py │ ├── simulation.py │ ├── data_generation.py │ └── cs_utils.py ├── tuning │ ├── __init__.py │ └── model_tuner.py ├── graphs │ ├── __init__.py │ ├── tuning_curve_graph.py │ └── kstep_graph.py ├── evaluation │ ├── __init__.py │ ├── evaluator.py │ ├── holdout_evaluator.py │ └── model_metrics.py ├── benchmarks │ ├── __init__.py │ ├── benchmark.py │ ├── halfcheetah.py │ ├── cartpole.py │ └── cartpole_v2.py ├── costs │ ├── __init__.py │ ├── quad_cost.py │ ├── gauss_reg_factory.py │ ├── cost_factory.py │ ├── sum_cost_factory.py │ ├── thresh_cost.py │ ├── quad_cost_factory.py │ ├── sum_cost.py │ └── cost.py ├── __init__.py ├── system.py ├── trajectory.py └── pipeline.py ├── docs ├── .gitignore ├── source │ ├── utils.rst │ ├── modules.rst │ ├── tasks.rst │ ├── graphs.rst │ ├── tuning.rst │ ├── evaluation.rst │ ├── benchmarks.rst │ ├── core.rst │ ├── sysid.rst │ ├── control.rst │ ├── autompc.rst │ └── costs.rst ├── index.rst ├── make.bat └── Makefile ├── examples ├── .gitignore ├── readme.md └── 1_Basics.ipynb ├── assets └── cached_tunes │ └── cartpole_tune_result.pkl ├── requirements.txt ├── setup.py ├── .gitignore ├── LICENSE └── README.md /tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /autompc/control/.gitignore: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /autompc/sysid/.gitignore: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docs/.gitignore: -------------------------------------------------------------------------------- 1 | _build/ 2 | -------------------------------------------------------------------------------- /autompc/tasks/__init__.py: -------------------------------------------------------------------------------- 1 | from .task import Task 2 | -------------------------------------------------------------------------------- /examples/.gitignore: -------------------------------------------------------------------------------- 1 | cache 2 | example_data 3 | out 4 | -------------------------------------------------------------------------------- /autompc/utils/__init__.py: -------------------------------------------------------------------------------- 1 | from .make_utils import * 2 | from .simulation import simulate 3 | -------------------------------------------------------------------------------- /autompc/tuning/__init__.py: -------------------------------------------------------------------------------- 1 | from .pipeline_tuner import PipelineTuner 2 | from .model_tuner import ModelTuner 3 | -------------------------------------------------------------------------------- /autompc/graphs/__init__.py: -------------------------------------------------------------------------------- 1 | from .kstep_graph import KstepPredAccGraph 2 | from .tuning_curve_graph import TuningCurveGraph 3 | -------------------------------------------------------------------------------- /autompc/evaluation/__init__.py: -------------------------------------------------------------------------------- 1 | from .evaluator import ModelEvaluator 2 | from .holdout_evaluator import HoldoutModelEvaluator 3 | -------------------------------------------------------------------------------- /docs/source/utils.rst: -------------------------------------------------------------------------------- 1 | utils package 2 | ============= 3 | 4 | simulate 5 | -------- 6 | .. autofunction:: autompc.utils.simulate 7 | 8 | -------------------------------------------------------------------------------- /assets/cached_tunes/cartpole_tune_result.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uiuc-iml/autompc/HEAD/assets/cached_tunes/cartpole_tune_result.pkl -------------------------------------------------------------------------------- /docs/source/modules.rst: -------------------------------------------------------------------------------- 1 | .. _modules: 2 | 3 | autompc 4 | ======= 5 | 6 | .. toctree:: 7 | :maxdepth: 4 8 | 9 | autompc 10 | -------------------------------------------------------------------------------- /docs/source/tasks.rst: -------------------------------------------------------------------------------- 1 | tasks package 2 | =============== 3 | 4 | The Task Class 5 | ^^^^^^^^^^^^^^ 6 | 7 | .. autoclass:: autompc.tasks.Task 8 | :members: 9 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | gpytorch~=1.2 2 | scipy~=1.7.0 3 | matplotlib~=3.1 4 | smac~=0.13 5 | numpy~=1.19.0 6 | pysindy~=1.0 7 | tqdm~=4.49 8 | ConfigSpace~=0.4 9 | scikit_learn~=0.24 10 | control~=0.9.0 11 | -------------------------------------------------------------------------------- /autompc/benchmarks/__init__.py: -------------------------------------------------------------------------------- 1 | from .benchmark import Benchmark 2 | from .cartpole import CartpoleSwingupBenchmark 3 | from .cartpole_v2 import CartpoleSwingupV2Benchmark 4 | from .halfcheetah import HalfcheetahBenchmark 5 | -------------------------------------------------------------------------------- /autompc/sysid/__init__.py: -------------------------------------------------------------------------------- 1 | from .arx import ARX, ARXFactory 2 | from .koopman import Koopman, KoopmanFactory 3 | from .sindy import SINDy, SINDyFactory 4 | #from .gp import GaussianProcess 5 | from .mlp import MLP, MLPFactory 6 | from .largegp import ApproximateGPModel, ApproximateGPModelFactory 7 | #from .linearize import LinearizedModel 8 | -------------------------------------------------------------------------------- /docs/source/graphs.rst: -------------------------------------------------------------------------------- 1 | graphs package 2 | ============== 3 | 4 | TuningCurveGraph 5 | --------------- 6 | .. autoclass:: autompc.graphs.TuningCurveGraph 7 | :members: __call__ 8 | 9 | KstepPredAccGraph 10 | ----------------- 11 | 12 | .. autoclass:: autompc.graphs.KstepPredAccGraph 13 | :members: __init__, add_model, __call___ 14 | -------------------------------------------------------------------------------- /autompc/costs/__init__.py: -------------------------------------------------------------------------------- 1 | from .quad_cost import QuadCost 2 | from .quad_cost_factory import QuadCostFactory 3 | from .gauss_reg_factory import GaussRegFactory 4 | from .thresh_cost import ThresholdCost, BoxThresholdCost 5 | from .sum_cost import SumCost 6 | from .sum_cost_factory import SumCostFactory 7 | from .cost import Cost 8 | from .cost_factory import CostFactory 9 | -------------------------------------------------------------------------------- /autompc/__init__.py: -------------------------------------------------------------------------------- 1 | print("Loading AutoMPC...") 2 | 3 | from .sysid.model import Model 4 | from .system import System 5 | from .control.controller import Controller 6 | from .trajectory import Trajectory, zeros, empty, extend 7 | from .tasks import Task 8 | from .utils import make_model, make_controller, simulate 9 | from .pipeline import Pipeline 10 | 11 | print("Finished loading AutoMPC") 12 | -------------------------------------------------------------------------------- /autompc/control/__init__.py: -------------------------------------------------------------------------------- 1 | from .lqr import LQRFactory, FiniteHorizonLQR, InfiniteHorizonLQR 2 | from .ilqr import IterativeLQR, IterativeLQRFactory 3 | try: 4 | from .nmpc import DirectTranscriptionController, DirectTranscriptionControllerFactory 5 | except ImportError: 6 | print("Missing optional dependency for NMPC") 7 | from .mppi import MPPI, MPPIFactory 8 | from .zero import ZeroController, ZeroControllerFactory 9 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | setup( 4 | name="autompc", 5 | version="0.0.1", 6 | description="A library for the creation and tuning of System ID+MPC pipelines", 7 | author="William Edwards", 8 | author_email="williamedwards314@gmail.com", 9 | url="https://github.com/williamedwards/autompc", 10 | packages=find_packages(include=["autompc", "autompc.*"]) 11 | ) 12 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | cache/* 3 | *.npz 4 | *.png 5 | *.pdf 6 | # Byte-compiled / optimized / DLL files 7 | __pycache__/ 8 | *.py[cod] 9 | *$py.class 10 | 11 | # C extensions 12 | *.so 13 | 14 | # Distribution / packaging 15 | .Python 16 | build/ 17 | develop-eggs/ 18 | dist/ 19 | downloads/ 20 | eggs/ 21 | .eggs/ 22 | lib/ 23 | lib64/ 24 | parts/ 25 | sdist/ 26 | var/ 27 | wheels/ 28 | share/python-wheels/ 29 | *.egg-info/ 30 | .installed.cfg 31 | *.egg 32 | MANIFEST 33 | -------------------------------------------------------------------------------- /autompc/utils/make_utils.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | def make_model(system, model, configuration, **kwargs): 4 | return model(system, **configuration.get_dictionary(), **kwargs) 5 | 6 | def make_transformer(system, transformer, configuration): 7 | return transformer(system, **configuration.get_dictionary()) 8 | 9 | def make_controller(system, task, model, controller, configuration, **kwargs): 10 | return controller(system, task, model, **configuration.get_dictionary(), 11 | **kwargs) 12 | -------------------------------------------------------------------------------- /docs/source/tuning.rst: -------------------------------------------------------------------------------- 1 | tuning package 2 | ============== 3 | 4 | Model Tuning 5 | ^^^^^^^^^^^^ 6 | 7 | ModelTuneResult 8 | --------------- 9 | .. autoclass:: autompc.tuning.model_tuner.ModelTuneResult 10 | 11 | ModelTuner 12 | ---------- 13 | 14 | .. autoclass:: autompc.tuning.ModelTuner 15 | :members: __init__, add_model_factory, run 16 | 17 | Pipeline Tuning 18 | ^^^^^^^^^^^^^^^ 19 | 20 | PipelineTuneResult 21 | ------------------ 22 | .. autoclass:: autompc.tuning.pipeline_tuner.PipelineTuneResult 23 | 24 | PipelineTuner 25 | ------------- 26 | .. autoclass:: autompc.tuning.PipelineTuner 27 | :members: __init__, run 28 | -------------------------------------------------------------------------------- /docs/source/evaluation.rst: -------------------------------------------------------------------------------- 1 | evaluation package 2 | ================== 3 | 4 | Base Classes 5 | ^^^^^^^^^^^^ 6 | 7 | Model Evaluator 8 | --------------- 9 | 10 | .. autoclass:: autompc.evaluation.ModelEvaluator 11 | :members: __init__, __call__ 12 | 13 | Evaluator Types 14 | ^^^^^^^^^^^^^^^ 15 | 16 | HoldoutModelEvaluator 17 | --------------------- 18 | 19 | .. autoclass:: autompc.evaluation.HoldoutModelEvaluator 20 | :members: __init__ 21 | 22 | Model Metrics 23 | ^^^^^^^^^^^^^ 24 | 25 | .. autofunction:: autompc.evaluation.model_metrics.get_model_rmse 26 | 27 | .. autofunction:: autompc.evaluation.model_metrics.get_model_rmsmens 28 | -------------------------------------------------------------------------------- /docs/source/benchmarks.rst: -------------------------------------------------------------------------------- 1 | benchmarks package 2 | ================== 3 | 4 | Base Classes 5 | ^^^^^^^^^^^^ 6 | 7 | Benchmark 8 | --------- 9 | .. autoclass:: autompc.benchmarks.Benchmark 10 | :members: 11 | 12 | 13 | Available Benchmarks 14 | ^^^^^^^^^^^^^^^^^^^^ 15 | 16 | CartpoleSwingupBenchmark 17 | ------------------------ 18 | .. autoclass:: autompc.benchmarks.CartpoleSwingupBenchmark 19 | :members: visualize 20 | 21 | CartpoleSwingupV2Benchmark 22 | -------------------------- 23 | .. autoclass:: autompc.benchmarks.CartpoleSwingupV2Benchmark 24 | :members: visualize 25 | 26 | HalfcheetahBenchmark 27 | -------------------- 28 | .. autoclass:: autompc.benchmarks.halfcheetah.HalfcheetahBenchmark 29 | :members: visualize 30 | -------------------------------------------------------------------------------- /docs/source/core.rst: -------------------------------------------------------------------------------- 1 | core classes 2 | ============ 3 | 4 | System 5 | ^^^^^^ 6 | .. autoclass:: autompc.System 7 | :members: __init__, controls, observations, ctrl_dim, obs_dim 8 | 9 | Trajectories 10 | ^^^^^^^^^^^^ 11 | 12 | Trajectory 13 | ---------- 14 | .. autoclass:: autompc.Trajectory 15 | :members: __init__, system, size, obs, ctrls 16 | 17 | TimeStep 18 | -------- 19 | .. autoclass:: autompc.trajectory.TimeStep 20 | 21 | zeros 22 | ----- 23 | .. autofunction:: autompc.zeros 24 | 25 | empty 26 | ----- 27 | .. autofunction:: autompc.empty 28 | 29 | extend 30 | ------ 31 | .. autofunction:: autompc.extend 32 | 33 | Pipeline 34 | ^^^^^^^^ 35 | .. autoclass:: autompc.Pipeline 36 | :members: __init__, get_configuration_space, __call__ 37 | -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | .. AutoMPC documentation master file, created by 2 | sphinx-quickstart on Mon Apr 27 18:29:36 2020. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to AutoMPC's documentation! 7 | =================================== 8 | 9 | Contents: 10 | 11 | .. toctree:: 12 | :maxdepth: 3 13 | 14 | source/core 15 | source/sysid 16 | source/control 17 | source/tasks 18 | source/costs 19 | source/evaluation 20 | source/tuning 21 | source/benchmarks 22 | source/graphs 23 | source/utils 24 | 25 | 26 | .. 27 | Indices and tables 28 | ================== 29 | 30 | * :ref:`genindex` 31 | * :doc:`source/modules` 32 | * :ref:`search` 33 | 34 | -------------------------------------------------------------------------------- /docs/source/sysid.rst: -------------------------------------------------------------------------------- 1 | sysid package 2 | ============= 3 | 4 | SysID Base Classes 5 | ------------------ 6 | 7 | The ModelFactory Class 8 | ^^^^^^^^^^^^^^^^^^^^^^^ 9 | 10 | .. autoclass:: autompc.sysid.model.ModelFactory 11 | :members: 12 | 13 | The Model Class 14 | ^^^^^^^^^^^^^^^ 15 | 16 | .. autoclass:: autompc.sysid.model.Model 17 | :members: 18 | 19 | 20 | Supported System ID Models 21 | -------------------------- 22 | 23 | Multi-layer Perceptron 24 | ^^^^^^^^^^^^^^^^^^^^^^ 25 | 26 | .. autoclass:: autompc.sysid.MLPFactory 27 | 28 | Sparse Identification of Nonlinear Dynamics (SINDy) 29 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 30 | 31 | .. autoclass:: autompc.sysid.SINDyFactory 32 | 33 | Autoregression (ARX) 34 | ^^^^^^^^^^^^^^^^^^^^ 35 | 36 | .. autoclass:: autompc.sysid.ARXFactory 37 | 38 | Koopman 39 | ^^^^^^^ 40 | 41 | .. autoclass:: autompc.sysid.KoopmanFactory 42 | 43 | Approximate Gaussian Process 44 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 45 | 46 | .. autoclass:: autompc.sysid.ApproximateGPModelFactory 47 | -------------------------------------------------------------------------------- /docs/source/control.rst: -------------------------------------------------------------------------------- 1 | control package 2 | =============== 3 | 4 | Control Base Classes 5 | -------------------- 6 | 7 | The ControllerFactory Class 8 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^ 9 | 10 | .. autoclass:: autompc.control.controller.ControllerFactory 11 | :members: __call__, get_configuration_space 12 | 13 | The Controller Class 14 | ^^^^^^^^^^^^^^^^^^^^ 15 | 16 | .. autoclass:: autompc.control.controller.Controller 17 | :members: 18 | 19 | 20 | Supported Controllers 21 | --------------------- 22 | 23 | Linear Quadratic Regulator (LQR) 24 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 25 | 26 | .. autoclass:: autompc.control.LQRFactory 27 | 28 | Iterative Linear Quadratic Regulator (iLQR) 29 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 30 | 31 | .. autoclass:: autompc.control.IterativeLQRFactory 32 | 33 | Direct Transcription (DT) 34 | ^^^^^^^^^^^^^^^^^^^^^^^^^ 35 | 36 | .. autoclass:: autompc.control.DirectTranscriptionControllerFactory 37 | 38 | Model Predictive Path Integral (MPPI) 39 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 40 | 41 | .. autoclass:: autompc.control.MPPIFactory 42 | 43 | Zero Controller 44 | ^^^^^^^^^^^^^^^ 45 | .. autoclass:: autompc.control.ZeroControllerFactory 46 | -------------------------------------------------------------------------------- /autompc/graphs/tuning_curve_graph.py: -------------------------------------------------------------------------------- 1 | from ..tuning.pipeline_tuner import PipelineTuneResult 2 | from ..tuning.model_tuner import ModelTuneResult 3 | 4 | class TuningCurveGraph: 5 | """ 6 | Graph tuning curve for either pipeline or model tuning 7 | result. 8 | """ 9 | def __call__(self, ax, tune_result): 10 | """ 11 | Parameters 12 | ---------- 13 | ax : matplotlib.axes.Axes 14 | Axes object on which to create graph 15 | tune_result : ModelTuneResult or PipelineTuneResult 16 | Tuning result to plot 17 | """ 18 | if isinstance(tune_result, PipelineTuneResult): 19 | if tune_result.inc_truedyn_costs is not None: 20 | ax.plot(tune_result.inc_truedyn_costs, label="True Dyn. Cost") 21 | ax.plot(tune_result.inc_costs, label="Surr. Cost") 22 | ax.set_xlabel("Tuning Iteration") 23 | ax.set_ylabel("Cost") 24 | ax.legend() 25 | elif isinstance(tune_result, ModelTuneResult): 26 | ax.plot(tune_result.inc_costs, label="Surr. Cost") 27 | ax.set_xlabel("Tuning Iteration") 28 | ax.set_ylabel("Model Error") 29 | -------------------------------------------------------------------------------- /docs/source/autompc.rst: -------------------------------------------------------------------------------- 1 | autompc package 2 | =============== 3 | 4 | .. 5 | #autompc.constraint module 6 | #------------------------- 7 | # 8 | #.. automodule:: autompc.constraint 9 | # :members: 10 | # :undoc-members: 11 | # :show-inheritance: 12 | # 13 | #autompc.cost module 14 | #------------------- 15 | # 16 | #.. automodule:: autompc.cost 17 | # :members: 18 | # :undoc-members: 19 | # :show-inheritance: 20 | # 21 | #autompc.model module 22 | #-------------------- 23 | # 24 | #.. automodule:: autompc.model 25 | # :members: 26 | # :undoc-members: 27 | # :show-inheritance: 28 | 29 | 30 | 31 | .. autompc.tasks module 32 | ------------------- 33 | 34 | .. automodule:: autompc.tasks 35 | :members: 36 | :undoc-members: 37 | :show-inheritance: 38 | 39 | autompc.sysid module 40 | -------------------- 41 | .. automodule:: autompc.sysid.mlp 42 | :members: 43 | :undoc-members: 44 | :show-inheritance: 45 | 46 | .. automodule:: autompc.sysid.sindy 47 | :members: 48 | :undoc-members: 49 | :show-inheritance: 50 | 51 | 52 | Module contents 53 | --------------- 54 | 55 | .. automodule:: autompc 56 | :members: 57 | :undoc-members: 58 | :show-inheritance: 59 | -------------------------------------------------------------------------------- /autompc/control/zero.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | from pdb import set_trace 4 | 5 | import numpy as np 6 | 7 | from ConfigSpace import ConfigurationSpace 8 | from ConfigSpace.hyperparameters import (UniformIntegerHyperparameter, 9 | CategoricalHyperparameter) 10 | import ConfigSpace.conditions as CSC 11 | 12 | from .controller import Controller, ControllerFactory 13 | 14 | class ZeroControllerFactory(ControllerFactory): 15 | """ 16 | The Zero Controller outputs all zero controls and is useful for debugging. 17 | 18 | Hyperparameters: *None* 19 | """ 20 | def __init__(self, *args, **kwargs): 21 | super().__init__(*args, **kwargs) 22 | self.Controller = ZeroController 23 | self.name = "ZeroController" 24 | 25 | def get_configuration_space(self): 26 | cs = CS.ConfigurationSpace() 27 | return cs 28 | 29 | class ZeroController(Controller): 30 | def __init__(self, system, task, model): 31 | super().__init__(system, task, model) 32 | 33 | @property 34 | def state_dim(self): 35 | return 0 36 | 37 | @staticmethod 38 | def is_compatible(system, task, model): 39 | return True 40 | 41 | def traj_to_state(self, traj): 42 | return np.zeros(0) 43 | 44 | def run(self, state, new_obs): 45 | 46 | return np.zeros(self.system.ctrl_dim), state 47 | -------------------------------------------------------------------------------- /docs/source/costs.rst: -------------------------------------------------------------------------------- 1 | costs package 2 | ============= 3 | 4 | Cost Base Classes 5 | ^^^^^^^^^^^^^^^^^ 6 | 7 | The CostFactory Class 8 | --------------------- 9 | .. autoclass:: autompc.costs.CostFactory 10 | :members: get_configuration_space, __call__ 11 | 12 | The Cost Class 13 | -------------- 14 | .. autoclass:: autompc.costs.Cost 15 | :members: __call__, get_cost_matrices, get_goal, eval_obs_cost, eval_obs_cost_diff, eval_obs_cost_hess, eval_ctrl_cost, eval_ctrl_cost_diff, eval_ctrl_cost_hess, eval_term_obs_cost, eval_cost_cost_diff, eval_term_obs_cost_hess, is_quad, is_convex, is_diff, is_twice_diff 16 | 17 | 18 | Cost Factory Classes 19 | ^^^^^^^^^^^^^^^^^^^^ 20 | 21 | QuadCostFactory 22 | --------------- 23 | .. autoclass:: autompc.costs.QuadCostFactory 24 | 25 | GaussRegFactory 26 | --------------- 27 | .. autoclass:: autompc.costs.GaussRegFactory 28 | 29 | SumCostFactory 30 | -------------- 31 | .. autoclass:: autompc.costs.SumCostFactory 32 | 33 | Cost Classes 34 | ^^^^^^^^^^^^ 35 | 36 | QuadCost 37 | -------- 38 | .. autoclass:: autompc.costs.QuadCost 39 | :members: __init__ 40 | 41 | SumCost 42 | ------ 43 | .. autoclass:: autompc.costs.SumCost 44 | :members: __init__ 45 | 46 | ThresholdCost 47 | ------------- 48 | .. autoclass:: autompc.costs.ThresholdCost 49 | :members: __init__ 50 | 51 | BoxThresholdCost 52 | ------------- 53 | .. autoclass:: autompc.costs.BoxThresholdCost 54 | :members: __init__ 55 | -------------------------------------------------------------------------------- /autompc/sysid/dummy_nonlinear.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as la 3 | import scipy.linalg as sla 4 | from pdb import set_trace 5 | from sklearn.linear_model import Lasso 6 | 7 | from .model import Model 8 | 9 | # Simulates 2-state system 10 | # x1[k+1] = x1[k] + x2[k]**3 11 | # x2[k+1] = x2[k] + u 12 | 13 | class DummyNonlinear(Model): 14 | def __init__(self, system): 15 | super().__init__(system) 16 | 17 | def state_dim(self): 18 | return 2 19 | 20 | def train(self, trajs): 21 | pass 22 | 23 | def traj_to_state(self, traj): 24 | state = np.zeros((2,)) 25 | state[:] = traj[-1].obs[:] 26 | return state[:] 27 | 28 | def update_state(state, new_obs, new_ctrl): 29 | return state[:] 30 | 31 | def pred(self, state, ctrl): 32 | u = ctrl[0] 33 | x1, x2 = state[0], state[1] 34 | xpred = np.array([x1 + x2**3, x2 + u]) 35 | 36 | return xpred 37 | 38 | def pred_diff(self, state, ctrl): 39 | u = ctrl[0] 40 | x1, x2 = state[0], state[1] 41 | xpred = np.array([x1 + x2**3, x2 + u]) 42 | grad1 = np.array([[1.0, 3 * x2 ** 2], [0., 1.]]) 43 | grad2 = np.array([[0.], [1.]]) 44 | return xpred, grad1, grad2 45 | 46 | @staticmethod 47 | def get_configuration_space(system): 48 | """ 49 | Returns the model configuration space. 50 | """ 51 | return None 52 | -------------------------------------------------------------------------------- /autompc/sysid/linearize.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | from .model import Model 4 | 5 | 6 | class LinearizedModel(Model): 7 | def __init__(self, system, x0, nonlinear_model): 8 | super().__init__(system) 9 | self.x0 = x0 10 | #self._model = NonlinearModel(system, **model_args) 11 | self._model = nonlinear_model 12 | _, self.A, self.B = nonlinear_model.pred_diff(x0, np.zeros(system.ctrl_dim)) 13 | 14 | @property 15 | def state_dim(self): 16 | return self._model.state_dim 17 | 18 | @property 19 | def state_dim(self): 20 | return self._model.state_dim 21 | 22 | @staticmethod 23 | def get_configuration_space(system): 24 | raise NotImplementedError 25 | 26 | def traj_to_state(self, traj): 27 | return self._model.traj_to_state(traj) 28 | 29 | def update_state(self, state, new_ctrl, new_obs): 30 | return np.copy(new_obs) 31 | 32 | def to_linear(self): 33 | return np.copy(self.A), np.copy(self.B) 34 | 35 | def pred(self, state, ctrl): 36 | xpred = self.A @ state + self.B @ ctrl 37 | 38 | def pred_diff(self, state, ctrl): 39 | xpred = self.A @ state + self.B @ ctrl 40 | 41 | def get_parameters(self): 42 | return {"A" : np.copy(self.A), 43 | "B" : np.copy(self.B)} 44 | 45 | def set_parameters(self, params): 46 | self.A = np.copy(params["A"]) 47 | self.B = np.copy(params["B"]) 48 | -------------------------------------------------------------------------------- /examples/readme.md: -------------------------------------------------------------------------------- 1 | # Table of Contents 2 | 1. [Basics](https://htmlpreview.github.io/?https://github.com/williamedwards/autompc/blob/main/examples/1_Basics.html) [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/drive/11Bdkd5PzGbkhCz45Na0fKnTSl54uUTm_) 3 | 4 | 2. [Models](https://htmlpreview.github.io/?https://github.com/williamedwards/autompc/blob/main/examples/2_Models.html) [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/drive/1-aNbehmk6aojCWz8N2aq1ap13-EdHnMO) 5 | 6 | 3. [Controllers and Tasks](https://htmlpreview.github.io/?https://github.com/williamedwards/autompc/blob/main/examples/3_Controllers_and_Tasks.html) [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/drive/1Iztden5SqagUHvBvco9g8cylhyXIJJV4) 7 | 8 | 4. [Factories and Pipelines](https://htmlpreview.github.io/?https://github.com/williamedwards/autompc/blob/main/examples/4_Factories_and_Pipelines.html) [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/drive/1W85uCa5hyCnR7O9pBgZZX329CUJYeNoB) 9 | 10 | 5. [Tuning](https://htmlpreview.github.io/?https://github.com/williamedwards/autompc/blob/main/examples/5_Tuning.html) [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/drive/1H-yjs52jT6PMu1wW-esrR2d5MuOVDPrH) 11 | -------------------------------------------------------------------------------- /autompc/sysid/dummy_linear.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from pdb import set_trace 3 | 4 | from .model import Model 5 | import ConfigSpace as CS 6 | import ConfigSpace.hyperparameters as CSH 7 | import ConfigSpace.conditions as CSC 8 | 9 | class DummyLinear(Model): 10 | def __init__(self, system, A, B): 11 | super().__init__(system) 12 | self.A = A 13 | self.B = B 14 | 15 | @staticmethod 16 | def get_configuration_space(system): 17 | cs = CS.ConfigurationSpace() 18 | return cs 19 | 20 | def traj_to_state(self, traj): 21 | return traj[-1].obs[:] 22 | 23 | def update_state(self, state, new_ctrl, new_obs): 24 | return np.copy(new_obs) 25 | 26 | @property 27 | def state_dim(self): 28 | return self.system.obs_dim 29 | 30 | def train(self, trajs): 31 | pass 32 | 33 | def pred(self, state, ctrl): 34 | xpred = self.A @ state + self.B @ ctrl 35 | return xpred 36 | 37 | def pred_diff(self, state, ctrl): 38 | xpred = self.A @ state + self.B @ ctrl 39 | 40 | return xpred, np.copy(self.A), np.copy(self.B) 41 | 42 | def to_linear(self): 43 | return np.copy(self.A), np.copy(self.B) 44 | 45 | def get_parameters(self): 46 | return {"A" : np.copy(self.A), 47 | "B" : np.copy(self.B)} 48 | 49 | def set_parameters(self, params): 50 | self.A = np.copy(params["A"]) 51 | self.B = np.copy(params["B"]) 52 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Univerisy ot Illinois 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /autompc/costs/quad_cost.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards, (wre2@illinois.edu) 2 | 3 | import numpy as np 4 | 5 | from .cost import Cost 6 | 7 | class QuadCost(Cost): 8 | def __init__(self, system, Q, R, F=None, goal=None): 9 | """ 10 | Create quadratic cost. 11 | 12 | Parameters 13 | ---------- 14 | system : System 15 | System for cost 16 | 17 | Q : numpy array of shape (self.obs_dim, self.obs_dim) 18 | Observation cost matrix 19 | 20 | R : numpy array of shape (self.ctrl_dim, self.ctrl_dim) 21 | Control cost matrix 22 | 23 | F : numpy array of shape (self.ctrl_dim, self.ctrl_dim) 24 | Terminal observation cost matrix 25 | 26 | goal : numpy array of shape self.obs_dim 27 | Goal state. Default is zero state 28 | """ 29 | super().__init__(system) 30 | if Q.shape != (system.obs_dim, system.obs_dim): 31 | raise ValueError("Q is the wrong shape") 32 | if R.shape != (system.ctrl_dim, system.ctrl_dim): 33 | raise ValueError("R is the wrong shape") 34 | if not F is None: 35 | if F.shape != (system.obs_dim, system.obs_dim): 36 | raise ValueError("F is the wrong shape") 37 | else: 38 | F = np.zeros((system.obs_dim, system.obs_dim)) 39 | 40 | self._Q = np.copy(Q) 41 | self._R = np.copy(R) 42 | self._F = np.copy(F) 43 | if goal is None: 44 | goal = np.zeros(system.obs_dim) 45 | self._goal = np.copy(goal) 46 | 47 | self._is_quad = True 48 | self._is_convex = True 49 | self._is_diff = True 50 | self._is_twice_diff = True 51 | self._has_goal = True 52 | -------------------------------------------------------------------------------- /autompc/costs/gauss_reg_factory.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-01-24 2 | 3 | # Internal library includes 4 | from .cost_factory import CostFactory 5 | from . import QuadCost 6 | 7 | # External library includes 8 | import numpy as np 9 | import numpy.linalg as la 10 | import ConfigSpace as CS 11 | import ConfigSpace.hyperparameters as CSH 12 | import ConfigSpace.conditions as CSC 13 | 14 | class GaussRegFactory(CostFactory): 15 | """ 16 | Cost factory for Gaussian regularization cost. This cost encourages the controller 17 | to stick close to the distribution of the training set, and is typically used in 18 | combination with another cost function. The factory returns a quadratic cost 19 | with :math:`Q= w \\Sigma_x^{-1}` and goal = :math:`\mu_x`. 20 | 21 | Hyperparameters: 22 | - *reg_weight* (float, Lower: 10^-3, Upper: 10^4): Weight of regularization term. 23 | """ 24 | def __init__(self, system): 25 | super().__init__(system) 26 | 27 | def get_configuration_space(self): 28 | cs = CS.ConfigurationSpace() 29 | reg_weight = CSH.UniformFloatHyperparameter("reg_weight", 30 | lower=1e-3, upper=1e4, default_value=1.0, log=True) 31 | cs.add_hyperparameter(reg_weight) 32 | return cs 33 | 34 | def is_compatible(self, system, task, Model): 35 | return True 36 | 37 | def __call__(self, cfg, task, trajs): 38 | X = np.concatenate([traj.obs[:,:] for traj in trajs]) 39 | mean = np.mean(X, axis=0) 40 | cov = np.cov(X, rowvar=0) 41 | Q = cfg["reg_weight"] * la.inv(cov) 42 | F = np.zeros_like(Q) 43 | R = np.zeros((self.system.ctrl_dim, self.system.ctrl_dim)) 44 | 45 | return QuadCost(self.system, Q, R, F, goal=mean) 46 | -------------------------------------------------------------------------------- /autompc/costs/cost_factory.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-01-24 2 | 3 | from abc import ABC, abstractmethod 4 | from pdb import set_trace 5 | 6 | class CostFactory(ABC): 7 | """ 8 | The CostFactory class constructs cost objects and contains information 9 | about hyperparameter information. 10 | """ 11 | 12 | def __init__(self, system): 13 | """ 14 | Consctruct CostFactory. 15 | 16 | Parameters 17 | ---------- 18 | system : System 19 | Robot system for cost factory 20 | """ 21 | self.system = system 22 | 23 | @abstractmethod 24 | def get_configuration_space(self): 25 | """ 26 | Returns ConfigurationSpace for cost factory. 27 | """ 28 | raise NotImplementedError 29 | 30 | # @abstractmethod 31 | # def is_compatible(self, system, task, Model): 32 | # raise NotImplementedError 33 | 34 | @abstractmethod 35 | def __call__(self, cfg, task, trajs): 36 | """ 37 | Build Cost according to configuration. 38 | 39 | Parameters 40 | ---------- 41 | cfg : Configuration 42 | Cost hyperparameter configuration 43 | 44 | task : Task 45 | Input task 46 | 47 | trajs : List of Trajectory 48 | Trajectory training set. This is mostly used 49 | for regularization cost terms and is not required by 50 | all CostFactories. If not required, None can be 51 | passed instead. 52 | """ 53 | raise NotImplementedError 54 | 55 | def __add__(self, other): 56 | from .sum_cost_factory import SumCostFactory 57 | if isinstance(other, SumCostFactory): 58 | return other.__radd__(self) 59 | else: 60 | return SumCostFactory(self.system, [self, other]) 61 | -------------------------------------------------------------------------------- /autompc/benchmarks/benchmark.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-01-08 2 | 3 | # Standard library includes 4 | from abc import ABC, abstractmethod 5 | 6 | # External library includes 7 | import numpy as np 8 | 9 | class Benchmark(ABC): 10 | """ 11 | Represents a Benchmark for testing AutoMPC, including the sytem, 12 | task, and a method of generating data. 13 | """ 14 | def __init__(self, name, system, task, data_gen_method): 15 | self.name = name 16 | self.system = system 17 | self.task = task 18 | self._data_gen_method = data_gen_method 19 | 20 | @abstractmethod 21 | def dynamics(self, x, u): 22 | """ 23 | Benchmark dynamics 24 | 25 | Parameters 26 | ---------- 27 | x : np array of size self.system.obs_dim 28 | Current observation 29 | 30 | u : np array of size self.system.ctrl_dim 31 | Control input 32 | 33 | Returns 34 | ------- 35 | xnew : np array of size self.system.obs_dim 36 | New observation. 37 | """ 38 | raise NotImplementedError 39 | 40 | @abstractmethod 41 | def gen_trajs(self, seed, n_trajs, traj_len=None): 42 | """ 43 | Generate trajectories. 44 | 45 | Parameters 46 | ---------- 47 | seed : int 48 | Seed for trajectory generation 49 | 50 | n_trajs : int 51 | Number of trajectories to generate 52 | 53 | traj_len : int 54 | Length of trajectories to generate. Default varies 55 | by benchmark. 56 | 57 | Returns 58 | ------- 59 | : List of Trajectory 60 | Benchmark training set 61 | """ 62 | raise NotImplementedError 63 | 64 | @staticmethod 65 | @abstractmethod 66 | def data_gen_methods(self): 67 | """ 68 | List available data generation methods 69 | 70 | Returns 71 | ------- 72 | : List of strings 73 | """ 74 | raise NotImplementedError 75 | -------------------------------------------------------------------------------- /autompc/utils/simulation.py: -------------------------------------------------------------------------------- 1 | # Standard library library 2 | import sys 3 | 4 | # Internal library includes 5 | from .. import zeros, extend 6 | 7 | # External library includes 8 | import numpy as np 9 | from tqdm import tqdm 10 | 11 | def simulate(controller, init_obs, term_cond=None, dynamics=None, sim_model=None, max_steps=10000, silent=False): 12 | """ 13 | Simulate a controller with respect to a dynamics function or simulation model. 14 | 15 | Parameters 16 | ---------- 17 | controller : Controller 18 | Controller to simulate 19 | 20 | init_obs : numpy array of size controller.system.obs_dim 21 | Initial observation 22 | 23 | term_cond : Function Trajectory -> bool 24 | Function which returns true when termination condition is met. 25 | 26 | dynamics : Function obs, control -> newobs 27 | Function defining system dynamics 28 | 29 | sim_model : Model 30 | Simulation model. Used when dynamics is None 31 | 32 | max_steps : int 33 | Maximum number of simulation steps allowed. Default is 10000. 34 | 35 | silent : bool 36 | Suppress output if True. 37 | """ 38 | if dynamics is None and sim_model is None: 39 | raise ValueError("Must specify dynamics function or simulation model") 40 | 41 | sim_traj = zeros(controller.system, 1) 42 | x = np.copy(init_obs) 43 | sim_traj[0].obs[:] = x 44 | 45 | constate = controller.traj_to_state(sim_traj) 46 | if dynamics is None: 47 | simstate = sim_model.traj_to_state(sim_traj) 48 | if silent: 49 | itr = range(max_steps) 50 | else: 51 | itr = tqdm(range(max_steps), file=sys.stdout) 52 | for _ in itr: 53 | u, constate = controller.run(constate, sim_traj[-1].obs) 54 | if dynamics is None: 55 | simstate = sim_model.pred(simstate, u) 56 | x = simstate[:controller.system.obs_dim] 57 | else: 58 | x = dynamics(x, u) 59 | sim_traj[-1].ctrl[:] = u 60 | sim_traj = extend(sim_traj, [x], 61 | np.zeros((1, controller.system.ctrl_dim))) 62 | if term_cond is not None and term_cond(sim_traj): 63 | break 64 | return sim_traj 65 | -------------------------------------------------------------------------------- /autompc/evaluation/evaluator.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | from abc import ABC, abstractmethod 4 | from collections import defaultdict 5 | from .model_metrics import get_model_rmse 6 | 7 | class ModelEvaluator(ABC): 8 | """ 9 | The ModelEvaluator class evaluates models by prediction accuracy. 10 | """ 11 | def __init__(self, system, trajs, metric, rng, horizon=1): 12 | """ 13 | Parameters 14 | ---------- 15 | system : System 16 | System for which prediction accuracy is evaluated 17 | trajs : List of Trajectory 18 | Trajectories to be used for evaluation 19 | metric : string or function (model, [Trajectory] -> float) 20 | Metric which evaluates the model against a set of trajectories. 21 | If string, one of "rmse", "rmsmens". See `model_metrics` for 22 | more details. 23 | rng : np.random.Generator 24 | Random number generator used in evaluation 25 | horizon : int 26 | Prediction horizon used in certain metrics. Default is 1. 27 | """ 28 | self.system = system 29 | self.trajs = trajs 30 | self.rng = rng 31 | if isinstance(metric, str): 32 | if metric == "rmse": 33 | self.metric = lambda model, trajs: get_model_rmse(model, 34 | trajs, horizon=horizon) 35 | elif metric == "rmsmens": 36 | self.metric = lambda model, trajs: get_model_rmsmens(model, 37 | trajs, horizon=horizon) 38 | else: 39 | self.metric = metric 40 | 41 | @abstractmethod 42 | def __call__(self, model_factory, configuration): 43 | """ 44 | Accepts the model class and the configuration. 45 | 46 | Parameters 47 | ---------- 48 | model_factory : ModelFactory 49 | Model factory evaluate 50 | 51 | configuration : Configuration 52 | Hyperparameter configuration used to 53 | instantiate model 54 | 55 | Returns 56 | ------- 57 | score : float 58 | Evaluated score. 59 | """ 60 | raise NotImplementedError 61 | -------------------------------------------------------------------------------- /autompc/graphs/kstep_graph.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards 2 | 3 | from pdb import set_trace 4 | 5 | import numpy as np 6 | import numpy.linalg as la 7 | 8 | from ..evaluation.model_metrics import get_model_rmse, get_model_rmsmens 9 | 10 | class KstepPredAccGraph: 11 | """ 12 | Create k-step model prediction accuracy graph. 13 | """ 14 | def __init__(self, system, trajs, kmax, logscale=False, metric="rmse"): 15 | """ 16 | Parameters 17 | ---------- 18 | system : System 19 | System on which models are being evaluted 20 | 21 | trajs : List of Trajectory 22 | Evaluation trajectory set 23 | 24 | kmax : int 25 | Maximum horizon to evaluate 26 | 27 | logscale : bool 28 | Use log scale on y-axis if true 29 | 30 | metric : string 31 | Prediction accuracy metric to use. One of "rmse" or "rmsmens" 32 | """ 33 | self.kmax = kmax 34 | self.trajs = trajs 35 | self.logscale = logscale 36 | self.models = [] 37 | self.labels = [] 38 | 39 | if metric == "rmse": 40 | self.metric = get_model_rmse 41 | elif metric == "rmsmens": 42 | self.metric = get_model_rmsmens 43 | 44 | def add_model(self, model, label): 45 | """ 46 | Add a model for comparison 47 | 48 | Parameters 49 | ---------- 50 | model : Model 51 | Model to compare 52 | 53 | label : string 54 | Label for model 55 | """ 56 | self.models.append(model) 57 | self.labels.append(label) 58 | 59 | 60 | def __call__(self, fig, ax): 61 | """ 62 | Create graph. 63 | 64 | Parameters 65 | ---------- 66 | fig : matplotlib.figure.Figure 67 | Figure in which to create graph 68 | 69 | ax : matplotlib.axes.Axes 70 | Axes in which to create graph 71 | """ 72 | for model, label in zip(self.models, self.labels): 73 | rmses = [self.metric(model, self.trajs, horizon) 74 | for horizon in range(1, self.kmax)] 75 | ax.plot(rmses, label=label) 76 | 77 | ax.set_xlabel("Prediction Horizon") 78 | ax.set_ylabel("Prediction Error") 79 | if self.logscale: 80 | ax.set_yscale("log") 81 | 82 | ax.legend() 83 | -------------------------------------------------------------------------------- /autompc/costs/sum_cost_factory.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-01-24 2 | 3 | # Standard library includes 4 | from pdb import set_trace 5 | 6 | # Internal library includes 7 | from .cost_factory import CostFactory 8 | from .sum_cost import SumCost 9 | from ..utils.cs_utils import * 10 | from . import QuadCost 11 | 12 | # External library includes 13 | import numpy as np 14 | import ConfigSpace as CS 15 | import ConfigSpace.hyperparameters as CSH 16 | import ConfigSpace.conditions as CSC 17 | 18 | class SumCostFactory(CostFactory): 19 | """ 20 | A factory which produces sums of other cost terms. A SumCostFactory 21 | can be crated by combining other costfactories with the `+` operator. 22 | """ 23 | def __init__(self, system, factories): 24 | super().__init__(system) 25 | self._factories = factories[:] 26 | 27 | @property 28 | def factories(self): 29 | return self._factories[:] 30 | 31 | def get_configuration_space(self, *args, **kwargs): 32 | cs = CS.ConfigurationSpace() 33 | for i, factory in enumerate(self.factories): 34 | _fact_cs = factory.get_configuration_space(*args, **kwargs) 35 | add_configuration_space(cs,"_sum_{}".format(i), _fact_cs) 36 | return cs 37 | 38 | def is_compatible(self, *args, **kwargs): 39 | for factory in self.factories: 40 | if not factory.is_compatible(*args, **kwargs): 41 | return False 42 | return True 43 | 44 | def __call__(self, cfg, task, trajs): 45 | costs = [] 46 | for i, factory in enumerate(self.factories): 47 | fact_cs = factory.get_configuration_space() 48 | fact_cfg = fact_cs.get_default_configuration() 49 | set_subspace_configuration(cfg, "_sum_{}".format(i), fact_cfg) 50 | cost = factory(fact_cfg, task, trajs) 51 | costs.append(cost) 52 | return sum(costs, SumCost(self.system, [])) 53 | 54 | def __add__(self, other): 55 | if isinstance(other, SumCostFactory): 56 | return SumCostFactory([*self.factories, *other.factories]) 57 | else: 58 | return SumCostFactory([*self.factories, other]) 59 | 60 | def __radd__(self, other): 61 | if isinstance(other, SumCostFactory): 62 | return SumCostFactory([*other.factories, *self.factories]) 63 | else: 64 | return SumCostFactory([other, *self.factories]) 65 | -------------------------------------------------------------------------------- /autompc/evaluation/holdout_evaluator.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | import math, time 4 | from pdb import set_trace 5 | import numpy as np 6 | 7 | from .evaluator import ModelEvaluator 8 | from .. import utils 9 | 10 | class HoldoutModelEvaluator(ModelEvaluator): 11 | """ 12 | Evaluate model prediction accuracy according to a holdout set. 13 | """ 14 | def __init__(self, *args, holdout_prop = 0.1, holdout_set=None, verbose=False, **kwargs): 15 | """ 16 | Parameters 17 | ---------- 18 | system : System 19 | System for which prediction accuracy is evaluated 20 | trajs : List of Trajectory 21 | Trajectories to be used for evaluation 22 | metric : string or function (model, [Trajectory] -> float) 23 | Metric which evaluates the model against a set of trajectories. 24 | If string, one of "rmse", "rmsmens". See `model_metrics` for 25 | more details. 26 | rng : np.random.Generator 27 | Random number generator used in evaluation 28 | horizon : int 29 | Prediction horizon used in certain metrics. Default is 1. 30 | holdout_prop : float 31 | Proportion of dataset to hold out for evaluation 32 | holdout_set : List of Trajectory 33 | This argument can be passed to explicitly set holdout set, rather 34 | than randomly selecting it. Pass None otherwise. 35 | verbose : bool 36 | Whether to print information during evaluation 37 | """ 38 | super().__init__(*args, **kwargs) 39 | self.verbose = verbose 40 | if holdout_set is None: 41 | holdout_size = round(holdout_prop * len(self.trajs)) 42 | holdout_indices = self.rng.choice(np.arange(len(self.trajs)), 43 | holdout_size, replace=False) 44 | self.holdout = [self.trajs[i] for i in sorted(holdout_indices)] 45 | else: 46 | self.holdout = holdout_set 47 | self.training_set = [] 48 | for traj in self.trajs: 49 | if traj not in self.holdout: 50 | self.training_set.append(traj) 51 | 52 | def __call__(self, model_factory, configuration): 53 | if self.verbose: 54 | print("Evaluating Configuration:") 55 | print(configuration) 56 | print("----") 57 | m = model_factory(configuration, self.training_set) 58 | 59 | metric_value = self.metric(m, self.holdout) 60 | 61 | return metric_value 62 | -------------------------------------------------------------------------------- /autompc/system.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | class System: 4 | """ 5 | The System object defines a robot system, including the size of the 6 | control and observation dimensions and the labels for each control 7 | and observation variable. 8 | """ 9 | def __init__(self, observations, controls, dt=None): 10 | """ 11 | Parameters 12 | ---------- 13 | observations : List of strings 14 | Name of each observation dimension 15 | 16 | controls : List of strings 17 | Name of each control dimension 18 | 19 | dt : float 20 | Optional: Data time step for the system. 21 | """ 22 | # Check inputs 23 | obs_set = set(observations) 24 | ctrl_set = set(controls) 25 | e = ValueError("Observation and control labels must be unique") 26 | if len(obs_set) != len(observations): 27 | raise e 28 | if len(ctrl_set) != len(controls): 29 | raise e 30 | if ctrl_set.intersection(obs_set): 31 | raise e 32 | 33 | self._controls = controls[:] 34 | self._observations = observations[:] 35 | 36 | self.dt = dt 37 | 38 | def __eq__(self, other): 39 | return ((self.controls == other.controls) 40 | and (self.observations == other.observations)) 41 | 42 | def __str__(self): 43 | observation_str = '{} observations'.format(len(self._observations)) if len(self._observations) > 4 else '['+','.join(self._observations)+']' 44 | control_str = '{} controls'.format(len(self._controls)) if len(self._controls) > 4 else '['+','.join(self._controls)+']' 45 | if self.dt is None: 46 | return '{}({},{})'.format(self.__class__.__name__,observation_str,control_str) 47 | else: 48 | dt_str = "dt={.3f}".format(self.dt) 49 | return '{}({},{},{})'.format(self.__class__.__name__,observation_str,control_str,dt_str) 50 | 51 | 52 | 53 | @property 54 | def controls(self): 55 | """ 56 | Names of each control dimension 57 | """ 58 | return self._controls[:] 59 | 60 | @property 61 | def observations(self): 62 | """ 63 | Names of each observation dimension 64 | """ 65 | return self._observations[:] 66 | 67 | @property 68 | def ctrl_dim(self): 69 | """ 70 | Size of control dimensions 71 | """ 72 | return len(self._controls) 73 | 74 | @property 75 | def obs_dim(self): 76 | """ 77 | Size of observation dimensions 78 | """ 79 | return len(self._observations) 80 | -------------------------------------------------------------------------------- /autompc/costs/thresh_cost.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards, (wre2@illinois.edu) 2 | 3 | import numpy as np 4 | import numpy.linalg as la 5 | 6 | from .cost import Cost 7 | 8 | class ThresholdCost(Cost): 9 | def __init__(self, system, goal, obs_range, threshold): 10 | """ 11 | Create threshold cost. Returns 1 for every time steps 12 | where :math:`||x - x_\\textrm{goal}||_\\infty > \\textrm{threshold}`. 13 | The check is performed only over the observation dimensions from 14 | obs_range[0] to obs_range[1]. 15 | """ 16 | super().__init__(system) 17 | self._goal = np.copy(goal) 18 | self._threshold = np.copy(threshold) 19 | self._obs_range = obs_range[:] 20 | 21 | self._is_quad = False 22 | self._is_convex = False 23 | self._is_diff = False 24 | self._is_twice_diff = False 25 | self._has_goal = True 26 | 27 | def eval_obs_cost(self, obs): 28 | if (la.norm(obs[self._obs_range[0]:self._obs_range[1]] - self._goal[self._obs_range[0]:self._obs_range[1]], np.inf) 29 | > self._threshold): 30 | return 1.0 31 | else: 32 | return 0.0 33 | 34 | def eval_ctrl_cost(self, ctrl): 35 | return 0.0 36 | 37 | def eval_term_obs_cost(self, obs): 38 | return 0.0 39 | 40 | class BoxThresholdCost(Cost): 41 | def __init__(self, system, limits, goal=None): 42 | """ 43 | Create Box threshold cost. Returns 1 for every time steps 44 | where observation is outisde of limits. 45 | 46 | Paramters 47 | --------- 48 | system : System 49 | System cost is computed for 50 | 51 | limits : numpy array of shape (system.obs_dim, 2) 52 | Upper and lower limits. Use +np.inf or -np.inf 53 | to allow certain dimensions unbounded. 54 | 55 | goal : numpy array of size system.obs_dim 56 | Goal state. Not used directly for computing cost, but 57 | may be used by downstream cost factories. 58 | """ 59 | super().__init__(system) 60 | self._limits = np.copy(limits) 61 | 62 | self._is_quad = False 63 | self._is_convex = False 64 | self._is_diff = False 65 | self._is_twice_diff = False 66 | 67 | if goal is None: 68 | self._has_goal = False 69 | else: 70 | self._goal = np.copy(goal) 71 | self._has_goal = True 72 | 73 | def eval_obs_cost(self, obs): 74 | if (obs < self._limits[:,0]).any() or (obs > self._limits[:,1]).any(): 75 | return 1.0 76 | else: 77 | return 0.0 78 | 79 | def eval_ctrl_cost(self, ctrl): 80 | return 0.0 81 | 82 | def eval_term_obs_cost(self, obs): 83 | return 0.0 84 | -------------------------------------------------------------------------------- /autompc/sysid/rnn.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | import numpy as np 4 | import numpy.linalg as la 5 | 6 | import torch 7 | 8 | from .model import Model, Hyper 9 | 10 | class Net(torch.nn.Module): 11 | def __init__(self, 12 | state_size, 13 | control_size, 14 | h_1 = 32, 15 | h_2 = 32): 16 | super(RnnNetwork, self).__init__() 17 | 18 | # TODO Initialize network layers parameters 19 | 20 | def forward(self, x, u, latent=None): 21 | #TODO Layers computation 22 | return xnew, latent 23 | 24 | def fit(X, y, batch_size, nb_epochs): 25 | optim = torch.optim.Adam(self.parameters()) 26 | for i in nb_epochs: 27 | # Compute loss 28 | loss.backwards() 29 | opt.step() 30 | opt.zero_grad() 31 | 32 | 33 | class ARX(Model): 34 | def __init__(self): 35 | # Initialize hyperparameters and parameters to default values 36 | pass 37 | 38 | def _get_training_arrays(self, trajs): 39 | #TODO 40 | pass 41 | 42 | 43 | def train(self, trajs): 44 | X, y = self._get_training_arrays(self, trajs) 45 | 46 | self.net = Net(trajs[0][0].shape[0], trajs[0][1].shape[0], 47 | h_1 = self.h_1, h_2 = self.h_2) 48 | self.net.fit(X, y, self.batch_size, self.nb_epochs) 49 | 50 | 51 | def __call__(self, xs, us, latent=None, ret_grad=False): 52 | if latent: 53 | xnew, latentnew = self.net.forward(xs[-1], us[-1], latent=latent) 54 | else: 55 | for x, u in zip(xs, us): 56 | xcurr, latent = self.net.forward(x, u, latent=latent) 57 | xnew, latentnew = xcurr, latent 58 | if ret_grad: 59 | #TODO compute xgrad, ugrad 60 | return xnew, latent, (xgrad, ugrad) 61 | else: 62 | return xnew, latent 63 | 64 | 65 | def get_hyper_options(self): 66 | return {"h_1" : (Hyper.int_range, (32, 256)), 67 | "h_2" : (Hyper.int_range, (32, 256)), 68 | "batch_size" : (Hyper.int_range(10, 100)), 69 | "nb_epochs" : (Hyper.int_range(10, 1000))} 70 | 71 | def get_hypers(self): 72 | return {"h_1" : self.h_1, 73 | "h_2" : self.h_2, 74 | "batch_size" : self.batch_size, 75 | "nb_epochs" : self.nb_epochs} 76 | 77 | def set_hypers(self, hypers): 78 | if "h_1" in hypers: 79 | self.h_1 = hypers["h_1"] 80 | if "h_2" in hypers: 81 | self.h_2 = hypers["h_2"] 82 | if "batch_size" in hypers: 83 | self.batch_size = hypers["batch_size"] 84 | if "nb_epochs" in hypers: 85 | self.nb_epochs = hypers["nb_epochs"] 86 | 87 | def get_parameters(self): 88 | return {"weights" : copy.deepcopy(self.net.state_dict())} 89 | 90 | def set_parameters(self, params): 91 | self.net.load_state_dict(params["weights"]) 92 | 93 | 94 | -------------------------------------------------------------------------------- /autompc/control/controller.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | from abc import ABC, abstractmethod 4 | from pdb import set_trace 5 | 6 | class ControllerFactory(ABC): 7 | """ 8 | The ControllerFactroy creates a controller based 9 | on a hyperparameter configuration. 10 | """ 11 | def __init__(self, system, **kwargs): 12 | self.system = system 13 | self.kwargs = kwargs 14 | 15 | def __call__(self, cfg, task, model): 16 | """ 17 | Returns initialized controller. 18 | 19 | Parameters 20 | ---------- 21 | cfg : Configuration 22 | Hyperparameter configuration for the controller 23 | 24 | task : Task 25 | Task which controller will solve 26 | 27 | model : Model 28 | System ID model to use for optimization 29 | """ 30 | controller_kwargs = cfg.get_dictionary() 31 | controller_kwargs.update(self.kwargs) 32 | controller = self.Controller(self.system, task, model, **controller_kwargs) 33 | return controller 34 | 35 | def get_configuration_space(self): 36 | """ 37 | Returns the controller ConfigurationSpace. 38 | """ 39 | raise NotImplementedError 40 | 41 | class Controller(ABC): 42 | def __init__(self, system, task, model): 43 | """ 44 | Initialize the controller. 45 | 46 | Parameters 47 | ---------- 48 | system : System 49 | Robot system to control 50 | 51 | task : Task 52 | Task which controller will solve 53 | 54 | model : Model 55 | System ID model to use for optimization 56 | """ 57 | self.system = system 58 | self.model = model 59 | self.task = task 60 | 61 | @abstractmethod 62 | def traj_to_state(self, traj): 63 | """ 64 | Parameters 65 | ---------- 66 | traj : Trajectory 67 | State and control history up to present time 68 | Returns 69 | ------- 70 | state : numpy array of size self.state_dim 71 | Corresponding controller state. This is frequently, 72 | but not always, equal to the underlying systme ID 73 | model state. 74 | """ 75 | raise NotImplementedError 76 | 77 | @abstractmethod 78 | def run(self, state, new_obs): 79 | """ 80 | Run the controller for a given time step 81 | 82 | Parameters 83 | ---------- 84 | state : numpy array of size self.state_dim 85 | Current controller state 86 | new_obs : numpy array of size self.system.obs_dim 87 | Current observation state. 88 | Returns 89 | ------- 90 | ctrl : numpy array of size self.system.ctrl_dim 91 | Next control input 92 | newstate : numpy array of size self.state_dim 93 | New controller state 94 | """ 95 | raise NotImplementedError 96 | 97 | def reset(self): 98 | """ 99 | Re-initialize the controller. For controllers which 100 | cache previous results to warm-start optimization, this 101 | will clear the cache. 102 | """ 103 | pass 104 | 105 | # @staticmethod 106 | # @abstractmethod 107 | # def is_compatible(system, task, model): 108 | # """ 109 | # Returns true if the controller is compatible with 110 | # the given system, model, and task. Returns false 111 | # otherwise. 112 | # """ 113 | # raise NotImplementedError 114 | 115 | @property 116 | @abstractmethod 117 | def state_dim(self): 118 | """ 119 | Returns the size of the controller state. 120 | """ 121 | raise NotImplementedError 122 | -------------------------------------------------------------------------------- /autompc/costs/quad_cost_factory.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-01-24 2 | 3 | # Internal library includes 4 | from .cost_factory import CostFactory 5 | from . import QuadCost 6 | 7 | # External library includes 8 | import numpy as np 9 | import ConfigSpace as CS 10 | import ConfigSpace.hyperparameters as CSH 11 | import ConfigSpace.conditions as CSC 12 | 13 | class QuadCostFactory(CostFactory): 14 | """ 15 | Factory to produce quadratic cost. This cost has the form 16 | 17 | .. math:: 18 | 19 | x_N^T F x_N + \\sum_{t=0}^{N} (x_t^T Q x_t + u_t^T R u_t) 20 | 21 | Parameters: 22 | - *goal* (numpy array of size system.obs_dim): Goal state. Default is 23 | 0 state. 24 | 25 | Hyperparameters: 26 | 27 | - * **x**_Q* (float, Lower: 10^-3, Upper: 10^4): Digaonal Q matrix value 28 | corresponding to observation dimension with label **x** 29 | - * **x**_R* (float, Lower: 10^-3, Upper: 10^4): Digaonal R matrix value 30 | corresponding to control dimension with label **x** 31 | - * **x**_F* (float, Lower: 10^-3, Upper: 10^4): Digaonal F matrix value 32 | corresponding to ovservation dimension with label **x** 33 | """ 34 | def __init__(self, system, goal=None): 35 | super().__init__(system) 36 | if goal is None: 37 | self.goal = None 38 | else: 39 | self.goal = goal[:] 40 | 41 | def get_configuration_space(self): 42 | cs = CS.ConfigurationSpace() 43 | for i, obsname in enumerate(self.system.observations): 44 | if self.goal is not None and np.isnan(self.goal[i]): 45 | continue 46 | obsgain = CSH.UniformFloatHyperparameter("{}_Q".format(obsname), 47 | lower=1e-3, upper=1e4, default_value=1.0, log=True) 48 | cs.add_hyperparameter(obsgain) 49 | for i, obsname in enumerate(self.system.observations): 50 | if self.goal is not None and np.isnan(self.goal[i]): 51 | continue 52 | obsgain = CSH.UniformFloatHyperparameter("{}_F".format(obsname), 53 | lower=1e-3, upper=1e4, default_value=1.0, log=True) 54 | cs.add_hyperparameter(obsgain) 55 | for ctrlname in self.system.controls: 56 | ctrlgain = CSH.UniformFloatHyperparameter("{}_R".format(ctrlname), 57 | lower=1e-3, upper=1e4, default_value=1.0, log=True) 58 | cs.add_hyperparameter(ctrlgain) 59 | return cs 60 | 61 | def is_compatible(self, system, task, Model): 62 | return task.get_cost().has_goal 63 | 64 | def __call__(self, cfg, task, trajs): 65 | if self.goal is None and task.get_cost().has_goal: 66 | goal = task.get_cost().get_goal() 67 | elif self.goal is not None: 68 | goal = self.goal 69 | else: 70 | raise ValueError("QuadCostFactory requires goal") 71 | 72 | Q = np.zeros((self.system.obs_dim,self.system.obs_dim)) 73 | F = np.zeros((self.system.obs_dim,self.system.obs_dim)) 74 | R = np.zeros((self.system.ctrl_dim,self.system.ctrl_dim)) 75 | for i, obsname in enumerate(self.system.observations): 76 | hyper_name = "{}_Q".format(obsname) 77 | if hyper_name in cfg: 78 | Q[i,i] = cfg[hyper_name] 79 | else: 80 | Q[i,i] = 0.0 81 | for i, obsname in enumerate(self.system.observations): 82 | hyper_name = "{}_F".format(obsname) 83 | if hyper_name in cfg: 84 | F[i,i] = cfg[hyper_name] 85 | else: 86 | F[i,i] = 0.0 87 | for i, ctrlname in enumerate(self.system.controls): 88 | hyper_name = "{}_R".format(ctrlname) 89 | if hyper_name in cfg: 90 | R[i,i] = cfg[hyper_name] 91 | else: 92 | R[i,i] = 0.0 93 | 94 | goal = np.nan_to_num(goal, nan=0.0) 95 | return QuadCost(self.system, Q, R, F, goal=goal) 96 | -------------------------------------------------------------------------------- /autompc/evaluation/model_metrics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from .. import zeros 3 | from pdb import set_trace 4 | 5 | 6 | def normalize(means, std, A): 7 | At = [] 8 | for i in range(A.shape[1]): 9 | At.append((A[:,i] - means[i]) / std[i]) 10 | return np.vstack(At).T 11 | 12 | def get_model_rmse(model, trajs, horizon=1): 13 | """ 14 | Computes (unnormalized) RMSE at fixed horizon 15 | 16 | Parameters 17 | ---------- 18 | model : Model 19 | Model class to consider 20 | 21 | trajs : List of Trajectories 22 | Trajectories on which to evaluate 23 | 24 | horizon : int 25 | Prediction horizon at which to evaluate. 26 | Default is 1. 27 | """ 28 | sqerrss = [] 29 | for traj in trajs: 30 | if hasattr(model, "traj_to_states"): 31 | state = model.traj_to_states(traj[:-horizon]) 32 | else: 33 | state = traj.obs[:-horizon, :] 34 | for k in range(horizon): 35 | state = model.pred_batch(state, traj.ctrls[k:-(horizon-k), :]) 36 | if hasattr(model, "traj_to_states"): 37 | state = state[:,:model.system.obs_dim] 38 | actual = traj.obs[horizon:] 39 | sqerrs = (state - actual) ** 2 40 | sqerrss.append(sqerrs) 41 | sqerrs = np.concatenate(sqerrss) 42 | rmse = np.sqrt(np.mean(sqerrs, axis=None)*trajs[0].system.obs_dim) 43 | return rmse 44 | 45 | def get_model_rmsmens(model, trajs, horiz=1): 46 | R""" 47 | Compute root mean squared model error, normalized step-wise (RMSMENS). 48 | 49 | Given a set of trajectories :math:`\mathcal{T}`, let :math:`x_{i,t} \in \mathbb{R}^n, u_{i,t} \in \mathbb{R}^m` denote the state and control input in the :math:`i^\textrm{th}` trajectory at time :math:`t`. 50 | Let :math:`L_i` denote the length of the :math:`i\textrm{th}` trajectory. Given a predictive model :math:`g`, let :math:`g(i,t,k)` give the prediction for :math:`x_{i,t+k}` given the 51 | states :math:`\mathbf{x}_{i,1:t}` and controls :math:`\mathbf{u}_{i,1:t+k-1}`. 52 | Let 53 | 54 | .. math:: 55 | \sigma = \textrm{std} \{ x_{i,t+1} - x_{i,t} \mid i=1,\ldots,\left|\mathcal{T}\right| 56 | t=1,\ldots,L_i-1 \} 57 | \textrm{,} 58 | 59 | where the mean and standard deviation are computed element-wise. 60 | 61 | Denote the :math:`k`-step Root Mean Squared Model Error, Normalized Step-wise, RMSMENS(:math:`k`), of :math:`g` with respect to :math:`\mathcal{T}` as 62 | 63 | .. math:: 64 | 65 | \sqrt{ 66 | \frac{1}{n} 67 | \left\lVert 68 | \frac{1}{\left|\mathcal{T}\right|} 69 | \sum_{i=1}^{\left|\mathcal{T}\right|} 70 | \frac{1}{L_i-k } 71 | \sum_{t=1}^{L_i-k}\frac{1}{\sigma^2} e(i,t,k)^2 72 | \right\rVert_1 73 | } 74 | \textrm{,} 75 | 76 | where 77 | 78 | .. math:: 79 | e(i,t,k) = g(i,t,k) - g(i,t,k-1) - (x_{i,t+k} - x_{i,t+k-1}) 80 | 81 | Parameters 82 | ---------- 83 | model : Model 84 | Model class to consider 85 | 86 | trajs : List of Trajectories 87 | Trajectories on which to evaluate 88 | 89 | horizon : int 90 | Prediction horizon at which to evaluate. 91 | Default is 1. 92 | """ 93 | dY = np.concatenate([traj.obs[1:,:] - traj.obs[:-1,:] for traj in trajs]) 94 | dy_means = np.mean(dY, axis=0) 95 | dy_std = np.std(dY, axis=0) 96 | 97 | sqerrss = [] 98 | for traj in trajs: 99 | state = traj.obs[:-horiz, :] 100 | for k in range(horiz): 101 | pstate = state 102 | state = model.pred_parallel(state, traj.ctrls[k:-(horiz-k), :]) 103 | pred_deltas = state - pstate 104 | act_deltas = traj.obs[horiz:] - traj.obs[horiz-1:-1] 105 | norm_pred_deltas = normalize(dy_means, dy_std, pred_deltas) 106 | norm_act_deltas = normalize(dy_means, dy_std, act_deltas) 107 | sqerrs = (norm_pred_deltas - norm_act_deltas) ** 2 108 | sqerrss.append(sqerrs) 109 | sqerrs = np.concatenate(sqerrss) 110 | rmse = np.sqrt(np.mean(sqerrs, axis=None)) 111 | return rmse 112 | -------------------------------------------------------------------------------- /autompc/costs/sum_cost.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | from collections.abc import Iterable 4 | 5 | import numpy as np 6 | 7 | from .cost import Cost 8 | 9 | class SumCost(Cost): 10 | def __init__(self, system, costs): 11 | """ 12 | A cost which is the sum of other cost terms. It can be created by combining 13 | other Cost objects with the `+` operator 14 | 15 | Parameters 16 | ---------- 17 | system : System 18 | System for the cost object. 19 | 20 | costs : List of Costs 21 | Cost objects to be summed. 22 | """ 23 | super().__init__(system) 24 | self._costs = costs 25 | 26 | @property 27 | def costs(self): 28 | return self._costs[:] 29 | 30 | def get_cost_matrices(self): 31 | if self.is_quad: 32 | Q = np.zeros((self.system.obs_dim, self.system.obs_dim)) 33 | F = np.zeros((self.system.obs_dim, self.system.obs_dim)) 34 | R = np.zeros((self.system.ctrl_dim, self.system.ctrl_dim)) 35 | 36 | for cost in self._costs: 37 | Q_, R_, F_ = cost.get_cost_matrices() 38 | Q += Q_ 39 | R += R_ 40 | F += F_ 41 | return Q, R, F 42 | else: 43 | raise NotImplementedError 44 | 45 | def get_goal(self): 46 | if self.has_goal: 47 | return self.costs[0] 48 | 49 | def _sum_results(self, arg, attr): 50 | results = [getattr(cost, attr)(arg) for cost in self.costs] 51 | if isinstance(results[0], Iterable): 52 | return [sum(vals) for vals in zip(*results)] 53 | else: 54 | return sum(results) 55 | 56 | def eval_obs_cost(self, obs): 57 | return self._sum_results(obs, "eval_obs_cost") 58 | 59 | def eval_obs_cost_diff(self, obs): 60 | return self._sum_results(obs, "eval_obs_cost_diff") 61 | 62 | def eval_obs_cost_hess(self, obs): 63 | return self._sum_results(obs, "eval_obs_cost_hess") 64 | 65 | def eval_ctrl_cost(self, ctrl): 66 | return self._sum_results(ctrl, "eval_ctrl_cost") 67 | 68 | def eval_ctrl_cost_diff(self, ctrl): 69 | return self._sum_results(ctrl, "eval_ctrl_cost_diff") 70 | 71 | def eval_ctrl_cost_hess(self, ctrl): 72 | return self._sum_results(ctrl, "eval_ctrl_cost_hess") 73 | 74 | def eval_term_obs_cost(self, obs): 75 | return self._sum_results(obs, "eval_term_obs_cost") 76 | 77 | def eval_term_obs_cost_diff(self, obs): 78 | return self._sum_results(obs, "eval_term_obs_cost_diff") 79 | 80 | def eval_term_obs_cost_hess(self, obs): 81 | return self._sum_results(obs, "eval_term_obs_cost_hess") 82 | 83 | @property 84 | def is_quad(self): 85 | if not self.costs[0].is_quad: 86 | return False 87 | goal = self.costs[0].get_goal() 88 | for cost in self.costs[1:]: 89 | if not cost.is_quad: 90 | return False 91 | if not (goal == cost.get_goal()).all(): 92 | return False 93 | return True 94 | 95 | @property 96 | def is_convex(self): 97 | for cost in self.costs: 98 | if not cost.is_convex: 99 | return False 100 | return True 101 | 102 | @property 103 | def is_diff(self): 104 | for cost in self.costs: 105 | if not cost.is_diff: 106 | return False 107 | return True 108 | 109 | @property 110 | def is_twice_diff(self): 111 | for cost in self.costs: 112 | if not cost.is_diff: 113 | return False 114 | return True 115 | 116 | @property 117 | def has_goal(self): 118 | if not self.costs[0].has_goal: 119 | return False 120 | goal = self.costs[0].get_goal() 121 | for cost in self.costs[1:]: 122 | if not cost.has_goal: 123 | return False 124 | if not (goal == cost.get_goal()).all(): 125 | return False 126 | return True 127 | 128 | def __add__(self, other): 129 | if isinstance(other, SumCost): 130 | return SumCost(self.system, [*self.costs, *other.costs]) 131 | else: 132 | return SumCost(self.system, [*self.costs, other]) 133 | 134 | def __radd__(self, other): 135 | if isinstance(other, SumCost): 136 | return SumCost(self.system, [*other.costs, *self.costs]) 137 | else: 138 | return SumCost(self.system, [other, *self.costs]) 139 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Welcome to AutoMPC, a library for automating system identification and model predictive control. 2 | AutoMPC can 3 | * Build SystemID models and Controllers 4 | * Evaluate and compare models and controllers 5 | * Tune controllers without requiring interactive access to the system 6 | * Provides a variety of controllers and optimizers 7 | 8 | To see AutoMPC in action, check out this [example](https://htmlpreview.github.io/?https://github.com/williamedwards/autompc/blob/main/examples/0_MainDemo.html) 9 | [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/drive/1CNil-Cq24AjVtoArgWOW0ZvLNB-zcyGW). 10 | 11 | **NOTE: we are doing a lot of development on the 0.2-dev Github branch and it is mostly stable. We plan to release it soon, and it has major API changes so please switch over as soon as you can!** 12 | 13 | ## Why AutoMPC? 14 | 15 | System ID and Model Predictive Control are powerful tools for building robot controllers, 16 | but getting them up and running can take a lot of engineering work. Achieving good 17 | performance typically requires careful selection of a number of hyperparameters, 18 | including the MPC horizon, the terms of the objective function, and the parameters 19 | of the System ID algorithm. AutoMPC automates the selection of these hyperparameters 20 | and provides a toolbox of algorithms to choose from. 21 | 22 | ## How does AutoMPC work? 23 | 24 | AutoMPC tunes hyperparameters for the System ID, Control Optimizer, and objective function 25 | using a dataset collected offline. In other words, AutoMPC does not need to interact 26 | with the robot during tuning. This is accomplished by initially training a *surrogate* 27 | dynamics model. During tuning, the surrogate dynamics are then used to simulate candidate 28 | controllers in order to evaluate closed-loop performance. 29 | 30 | For more details, see our [paper](https://motion.cs.illinois.edu/papers/ICRA2021_Edwards_AutoMPC.pdf) 31 | 32 | ## How to use AutoMPC? 33 | 34 | Check out our [main example](https://htmlpreview.github.io/?https://github.com/williamedwards/autompc/blob/main/examples/0_MainDemo.html) [![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/drive/1CNil-Cq24AjVtoArgWOW0ZvLNB-zcyGW) 35 | to see an overview of the AutoMPC workflow. 36 | 37 | If you are interested, check out our [detailed examples](examples/readme.md) for more information on how to use the different parts of AutoMPC. 38 | 39 | ## What algorithms does AutoMPC support? 40 | 41 | For System ID, AutoMPC supports 42 | * [Multi-layer Perceptrons](https://autompc.readthedocs.io/en/latest/source/sysid.html#multi-layer-perceptron) 43 | * [Sparse Identification of Nonlinear Dynamics (SINDy)](https://autompc.readthedocs.io/en/latest/source/sysid.html#sparse-identification-of-nonlinear-dynamics-sindy) 44 | * [Autoregression](https://autompc.readthedocs.io/en/latest/source/sysid.html#autoregression-arx) 45 | * [Koopman Operators](https://autompc.readthedocs.io/en/latest/source/sysid.html#koopman) 46 | * [Approximate Gaussian Processes](https://autompc.readthedocs.io/en/latest/source/sysid.html#approximate-gaussian-process) 47 | 48 | For control optimization, AutoMPC supports 49 | * [Linear Quadratic Regulator](https://autompc.readthedocs.io/en/latest/source/control.html#linear-quadratic-regulator-lqr) 50 | * [Iterative LQR](https://autompc.readthedocs.io/en/latest/source/control.html#iterative-linear-quadratic-regulator-ilqr) 51 | * [Direct Transcription](https://autompc.readthedocs.io/en/latest/source/control.html#direct-transcription-dt) 52 | * [Model Path Predictive Integral](https://autompc.readthedocs.io/en/latest/source/control.html#model-predictive-path-integral-mppi) 53 | 54 | AutoMPC is also extensible, so you can use our tuning process with your own System ID and control methods. We'd also welcome contributions 55 | of new algorithms to the package. 56 | 57 | ## Installation 58 | 59 | 1. Clone the repository 60 | 2. Install [PyTorch](https://pytorch.org/get-started/locally/) 61 | 3. (Optional) For certain benchmarks to work, install OpenAI [gym](https://gym.openai.com/) and [Mujoco](http://www.mujoco.org/) 62 | 4. (Optional) To use DirectTranscriptionController, install IPOPT solver and cyipopt binding. See [instructions](https://cyipopt.readthedocs.io/en/latest/install.html) 63 | 5. Run `pip install -r requirements.txt` 64 | 6. Run `pip install -e .` 65 | 66 | ## Documentation 67 | [Python API Reference](https://autompc.readthedocs.io). 68 | 69 | The documentation can also be built offline. This requires Sphinx to be installed, 70 | which can be done by running 71 | ``` 72 | pip install sphinx 73 | ``` 74 | 75 | To build or re-build the documentation, run the following command from the `docs/` subdirectory. 76 | ``` 77 | make html 78 | ``` 79 | 80 | The documentation will be produced in `docs/html`. 81 | -------------------------------------------------------------------------------- /autompc/benchmarks/halfcheetah.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-01-09 2 | 3 | # Standard library includes 4 | import sys, time 5 | 6 | # External library includes 7 | import numpy as np 8 | 9 | # Project includes 10 | from .benchmark import Benchmark 11 | from ..utils.data_generation import * 12 | from .. import System 13 | from ..tasks import Task 14 | from ..costs import Cost 15 | 16 | def viz_halfcheetah_traj(env, traj, repeat): 17 | for _ in range(repeat): 18 | env.reset() 19 | qpos = traj[0].obs[:9] 20 | qvel = traj[0].obs[9:] 21 | env.set_state(qpos, qvel) 22 | for i in range(len(traj)): 23 | u = traj[i].ctrl 24 | env.step(u) 25 | env.render() 26 | time.sleep(0.05) 27 | time.sleep(1) 28 | 29 | def halfcheetah_dynamics(env, x, u, n_frames=5): 30 | old_state = env.sim.get_state() 31 | old_qpos = old_state[1] 32 | qpos = x[:len(old_qpos)] 33 | qvel = x[len(old_qpos):] 34 | new_state = mujoco_py.MjSimState(old_state.time, qpos, qvel, 35 | old_state.act, old_state.udd_state) 36 | env.sim.set_state(new_state) 37 | #env.sim.forward() 38 | env.sim.data.ctrl[:] = u 39 | for _ in range(n_frames): 40 | env.sim.step() 41 | new_qpos = env.sim.data.qpos 42 | new_qvel = env.sim.data.qvel 43 | 44 | return np.concatenate([new_qpos, new_qvel]) 45 | 46 | class HalfcheetahCost(Cost): 47 | def __init__(self, env): 48 | self._is_quad = False 49 | self._is_convex = False 50 | self._is_diff = False 51 | self._is_twice_diff = False 52 | self._has_goal = False 53 | self.env = env 54 | 55 | def __call__(self, traj): 56 | cum_reward = 0.0 57 | for i in range(len(traj)-1): 58 | reward_ctrl = -0.1 * np.square(traj[i].ctrl).sum() 59 | reward_run = (traj[i+1, "x0"] - traj[i, "x0"]) / self.env.dt 60 | cum_reward += reward_ctrl + reward_run 61 | return 200 - cum_reward 62 | 63 | def eval_obs_cost(self): 64 | raise NotImplementedError 65 | 66 | def eval_term_obs_cost(self): 67 | raise NotImplementedError 68 | 69 | def eval_ctrl_cost(self): 70 | raise NotImplementedError 71 | 72 | def gen_trajs(env, system, num_trajs=1000, traj_len=1000, seed=42): 73 | rng = np.random.default_rng(seed) 74 | trajs = [] 75 | env.seed(int(rng.integers(1 << 30))) 76 | env.action_space.seed(int(rng.integers(1 << 30))) 77 | for i in range(num_trajs): 78 | init_obs = env.reset() 79 | traj = ampc.zeros(system, traj_len) 80 | traj[0].obs[:] = np.concatenate([[0], init_obs]) 81 | for j in range(1, traj_len): 82 | action = env.action_space.sample() 83 | traj[j-1].ctrl[:] = action 84 | #obs, reward, done, info = env.step(action) 85 | obs = halfcheetah_dynamics(traj[j-1].obs[:], action) 86 | traj[j].obs[:] = obs 87 | trajs.append(traj) 88 | return trajs 89 | 90 | 91 | class HalfcheetahBenchmark(Benchmark): 92 | """ 93 | This benchmark uses the OpenAI gym halfcheetah benchmark and is consistent with the 94 | experiments in the ICRA 2021 paper. The benchmark reuqires OpenAI gym and mujoco_py 95 | to be installed. The performance metric is 96 | :math:`200-R` where :math:`R` is the gym reward. 97 | """ 98 | def __init__(self, data_gen_method="uniform_random"): 99 | name = "halfcheetah" 100 | system = ampc.System([f"x{i}" for i in range(18)], [f"u{i}" for i in range(6)]) 101 | 102 | import gym, mujoco_py 103 | env = gym.make("HalfCheetah-v2") 104 | self.env = env 105 | 106 | system.dt = env.dt 107 | cost = HalfcheetahCost(env) 108 | task = Task(system) 109 | task.set_cost(cost) 110 | task.set_ctrl_bounds(env.action_space.low, env.action_space.high) 111 | init_obs = np.concatenate([env.init_qpos, env.init_qvel]) 112 | task.set_init_obs(init_obs) 113 | task.set_num_steps(200) 114 | 115 | 116 | super().__init__(name, system, task, data_gen_method) 117 | 118 | def dynamics(self, x, u): 119 | return halfcheetah_dynamics(self.env,x,u) 120 | 121 | def gen_trajs(self, seed, n_trajs, traj_len=200): 122 | return gen_trajs(self.env, self.system, n_trajs, traj_len, seed) 123 | 124 | def visualize(self, traj, repeat): 125 | """ 126 | Visualize the half-cheetah trajectory using Gym functions. 127 | 128 | Parameters 129 | ---------- 130 | traj : Trajectory 131 | Trajectory to visualize 132 | 133 | repeat : int 134 | Number of times to repeat trajectory in visualization 135 | """ 136 | viz_halfcheetah_traj(self.env, traj, repeat) 137 | 138 | @staticmethod 139 | def data_gen_methods(): 140 | return ["uniform_random"] 141 | -------------------------------------------------------------------------------- /autompc/utils/data_generation.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-01-09 2 | 3 | # Standard library includes 4 | import sys 5 | from pdb import set_trace 6 | 7 | # External library includes 8 | import numpy as np 9 | import autompc as ampc 10 | 11 | # Project includes 12 | 13 | def uniform_random_generate(system, task, dynamics, rng, init_min, init_max, 14 | traj_len, n_trajs): 15 | trajs = [] 16 | for _ in range(n_trajs): 17 | state0 = [rng.uniform(minval, maxval, 1)[0] for minval, maxval 18 | in zip(init_min, init_max)] 19 | y = state0[:] 20 | traj = ampc.zeros(system, traj_len) 21 | traj.obs[:] = y 22 | umin, umax = task.get_ctrl_bounds().T 23 | for i in range(traj_len): 24 | traj[i].obs[:] = y 25 | u = rng.uniform(umin, umax, system.ctrl_dim) 26 | y = dynamics(y, u) 27 | traj[i].ctrl[:] = u 28 | trajs.append(traj) 29 | return trajs 30 | 31 | def prbs_generate(system, task, dynamics, rng, init_min, init_max, 32 | traj_len, n_trajs, states, Nswitch): 33 | trajs = [] 34 | for _ in range(n_trajs): 35 | # Compute control sequence 36 | switches = rng.choice(traj_len, Nswitch) 37 | switches = np.concatenate([[0], switches, [traj_len]]) 38 | u = np.zeros(traj_len) 39 | for ps, ns in zip(switches[:-1], switches[1:]): 40 | u[ps:ns] = rng.choice(states) 41 | 42 | state0 = [rng.uniform(minval, maxval, 1)[0] for minval, maxval 43 | in zip(init_min, init_max)] 44 | y = state0[:] 45 | traj = ampc.zeros(system, traj_len) 46 | traj.obs[:] = y 47 | for i in range(traj_len): 48 | traj[i].obs[:] = y 49 | y = dynamics(y, u[i]) 50 | traj[i].ctrl[:] = u[i] 51 | trajs.append(traj) 52 | return trajs 53 | 54 | def random_walk_generate(system, task, dynamics, rng, init_min, init_max, walk_rate, 55 | traj_len, n_trajs): 56 | trajs = [] 57 | for _ in range(n_trajs): 58 | state0 = [rng.uniform(minval, maxval, 1)[0] for minval, maxval 59 | in zip(init_min, init_max)] 60 | y = state0[:] 61 | traj = ampc.zeros(system, traj_len) 62 | traj.obs[:] = y 63 | umin, umax = task.get_ctrl_bounds().T 64 | uamp = np.min([umin, umax]) 65 | u = rng.uniform(umin, umax, system.ctrl_dim) 66 | step_size = walk_rate * system.dt 67 | for i in range(traj_len): 68 | traj[i].obs[:] = y 69 | u += uamp * step_size * rng.uniform(-1, 1, system.ctrl_dim) 70 | u = np.clip(u, umin, umax) 71 | y = dynamics(y, u) 72 | traj[i].ctrl[:] = u 73 | trajs.append(traj) 74 | return trajs 75 | 76 | 77 | def periodic_control_generate(system, task, dynamics, rng, init_min, init_max, U_1, 78 | traj_len, n_trajs): 79 | trajs = [] 80 | periods = list(range(1, traj_len, max([1, traj_len // n_trajs]))) 81 | print("periods=", periods) 82 | for period in periods: 83 | state0 = [rng.uniform(minval, maxval, 1)[0] for minval, maxval 84 | in zip(init_min, init_max)] 85 | y = state0[:] 86 | traj = ampc.zeros(system, traj_len) 87 | traj.obs[:] = y 88 | umin, umax = task.get_ctrl_bounds().T 89 | uamp = np.min([umin, umax]) 90 | for i in range(traj_len): 91 | traj[i].obs[:] = y 92 | u = uamp * U_1 * np.cos(2 * np.pi * i / period) 93 | y = dynamics(y, u) 94 | traj[i].ctrl[:] = u 95 | trajs.append(traj) 96 | return trajs 97 | 98 | def multisine_generate(system, task, dynamics, rng, init_min, init_max, n_freqs, 99 | traj_len, n_trajs, abort_if=None): 100 | trajs = [] 101 | periods = list(range(1, traj_len, n_freqs)) 102 | umin, umax = task.get_ctrl_bounds().T 103 | uamp = (umax - umin) / 2 104 | umed = (umax + umin) / 2 105 | 106 | for _ in range(n_trajs): 107 | weights = [] 108 | for i in range(system.ctrl_dim): 109 | vals = rng.uniform(size=len(periods)-1) 110 | vals = np.concatenate([[0.0], np.sort(vals), [1.0]]) 111 | weight = vals[1:] - vals[:-1] 112 | weights.append(weight) 113 | weights = np.array(weights) 114 | phases = rng.uniform(0, 2*np.pi, len(periods)) 115 | 116 | state0 = [rng.uniform(minval, maxval, 1)[0] for minval, maxval 117 | in zip(init_min, init_max)] 118 | y = state0[:] 119 | traj = ampc.zeros(system, traj_len) 120 | traj.obs[:] = y 121 | umin, umax = task.get_ctrl_bounds().T 122 | for i in range(traj_len): 123 | traj[i].obs[:] = y 124 | u = np.zeros(system.ctrl_dim) 125 | for j, period in enumerate(periods): 126 | u += weights[:,j] * np.cos(2 * np.pi * i / period + phases[j]) 127 | u = uamp * u + umed 128 | y = dynamics(y, u) 129 | traj[i].ctrl[:] = u 130 | if not abort_if is None and abort_if(y): 131 | traj = traj[:i] 132 | break 133 | trajs.append(traj) 134 | return trajs 135 | -------------------------------------------------------------------------------- /autompc/sysid/basis_funcs.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-02-07 2 | 3 | import numpy as np 4 | import inspect 5 | from pdb import set_trace 6 | from collections import namedtuple 7 | 8 | BasisFunction = namedtuple("BasisFunction", ["n_args", "func", "grad_func", "name_func"]) 9 | 10 | def get_constant_basis_func(): 11 | return BasisFunction(n_args=0, 12 | func = lambda : 1, 13 | grad_func = lambda : [0], 14 | name_func = lambda : "") 15 | 16 | def get_identity_basis_func(): 17 | return BasisFunction(n_args=1, 18 | func = lambda x : x, 19 | grad_func = lambda x : [1], 20 | name_func = lambda x : x) 21 | 22 | def get_poly_basis_func(degree): 23 | return BasisFunction(n_args=1, 24 | func = lambda x : x**degree, 25 | grad_func = lambda x : [x**(degree-1)], 26 | name_func = lambda x : "{}**{}".format(x,degree)) 27 | 28 | def get_cross_term_basis_funcs(degree): 29 | bfuncs = [] 30 | exponents = np.mgrid[tuple(slice(degree) for _ in range(degree))] 31 | exponents = exponents.reshape((degree, -1)) 32 | used_exps = set() 33 | for exp in exponents.T: 34 | if sum(exp) != degree: 35 | continue 36 | trimmed_exp = tuple(e for e in exp if e > 0) 37 | if trimmed_exp in used_exps: 38 | continue 39 | used_exps.add(trimmed_exp) 40 | n_args = len(trimmed_exp) 41 | arg_str = ", ".join(["x{}".format(i) for i in range(n_args)]) 42 | def func_(*args, tr): 43 | val = 1.0 44 | for arg, exp in zip(args, tr): 45 | val *= arg**exp 46 | return val 47 | 48 | if n_args == 1: 49 | func = lambda x0, *, tr=trimmed_exp: func_(x0, tr=tr) 50 | elif n_args == 2: 51 | func = lambda x0, x1, *, tr=trimmed_exp: func_(x0, x1, tr=tr) 52 | elif n_args == 3: 53 | func = lambda x0, x1, x2, *, tr=trimmed_exp: func_(x0, x1, x2, tr=tr) 54 | elif n_args == 4: 55 | func = lambda x0, x1, x2, x3, *, tr=trimmed_exp: func_(x0, x1, x2, x3, tr=tr) 56 | elif n_args == 5: 57 | func = lambda x0, x1, x2, x3, x4, *, tr=trimmed_exp: func_(x0, x1, x2, \ 58 | x3, x4, tr=tr) 59 | elif n_args == 6: 60 | func = lambda x0, x1, x2, x3, x4, x5, *, tr=trimmed_exp: func_(x0, x1, \ 61 | x2, x3, x4, x5, tr=tr) 62 | elif n_args == 7: 63 | func = lambda x0, x1, x2, x3, x4, x5, x6, *, tr=trimmed_exp: func_(x0, x1, \ 64 | x2, x3, x4, x5, x6, tr=tr) 65 | elif n_args == 8: 66 | func = lambda x0, x1, x2, x3, x4, x5, x6, x7, *,tr=trimmed_exp: func_(x0, x1, \ 67 | x2, x3, x4, x5, x6, x7, tr=tr) 68 | elif n_args == 9: 69 | func = lambda x0, x1, x2, x3, x4, x5, x6, x7, x8,*,tr=trimmed_exp: func_(x0, \ 70 | x1, x2, x3, x4, x5, x6, x7, x8, tr=tr) 71 | elif n_args == 10: 72 | func = lambda x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, *, tr=trimmed_exp: \ 73 | func_(x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, tr=tr) 74 | else: 75 | raise ValueError("n_args > 10") 76 | 77 | def grad_func(*args, trimmed_exp=trimmed_exp): 78 | grads = [] 79 | for i in range(len(args)): 80 | val = 1.0 81 | for j, (arg, exp) in enumerate(zip(args, trimmed_exp)): 82 | if i != j: 83 | val *= arg**exp 84 | else: 85 | val *= exp * arg**(exp-1) 86 | grads.append(val) 87 | return np.array(grads) 88 | def name_func(*args, trimmed_exp=trimmed_exp): 89 | name = "" 90 | for arg, exp in zip(args, trimmed_exp): 91 | name += "{}^{} ".format(arg, exp) 92 | return name 93 | bfuncs.append(BasisFunction(n_args = n_args, func = func, 94 | grad_func=grad_func, name_func=name_func)) 95 | return bfuncs 96 | 97 | def get_trig_basis_funcs(freq): 98 | sin_bfunc = BasisFunction(n_args=1, 99 | func = lambda x : np.sin(freq * x), 100 | grad_func = lambda x : [freq * np.cos(freq * x)], 101 | name_func = lambda x : "sin({} {})".format(freq, x)) 102 | cos_bfunc = BasisFunction(n_args=1, 103 | func = lambda x : np.cos(freq * x), 104 | grad_func = lambda x : [-freq * np.sin(freq * x)], 105 | name_func = lambda x : "cos({} {})".format(freq, x)) 106 | return [sin_bfunc, cos_bfunc] 107 | 108 | def get_trig_interaction_terms(freq): 109 | sin_bfunc = BasisFunction(n_args=2, 110 | func = lambda x,y : x * np.sin(freq * y), 111 | grad_func = lambda x,y : [np.sin(freq * y), x * freq * np.cos(freq * y)], 112 | name_func = lambda x,y : "{} sin({} {})".format(x, freq, y)) 113 | sin_bfunc2 = BasisFunction(n_args=2, 114 | func = lambda y,x : x * np.sin(freq * y), 115 | grad_func = lambda y,x : [x * freq * np.cos(freq * y), np.sin(freq * y)], 116 | name_func = lambda y,x : "{} sin({} {})".format(x, freq, y)) 117 | cos_bfunc = BasisFunction(n_args=2, 118 | func = lambda x,y : x * np.cos(freq * y), 119 | grad_func = lambda x,y : [np.cos(freq * y), x * -freq * np.sin(freq * y)], 120 | name_func = lambda x,y : "{} cos({} {})".format(x, freq, y)) 121 | cos_bfunc2 = BasisFunction(n_args=2, 122 | func = lambda y,x : x * np.cos(freq * y), 123 | grad_func = lambda y,x : [x * -freq * np.sin(freq * y), np.cos(freq * y)], 124 | name_func = lambda y,x : "{} cos({} {})".format(x, freq, y)) 125 | return sin_bfunc, sin_bfunc2, cos_bfunc, cos_bfunc2 126 | 127 | -------------------------------------------------------------------------------- /autompc/sysid/stable_koopman.py: -------------------------------------------------------------------------------- 1 | __author__ = "Giorgos Mamakoukas" 2 | __copyright__ = "Copyright (C) 2004 Giorgos Mamakoukas" 3 | 4 | 5 | from pdb import set_trace 6 | 7 | import numpy as np 8 | from scipy.linalg import polar, pinv2, solve_discrete_lyapunov, sqrtm 9 | import math 10 | from scipy import io 11 | 12 | 13 | def projectPSD(Q, epsilon = 0, delta = math.inf): 14 | Q = (Q+Q.T)/2 15 | [e, V] = np.linalg.eig(Q) 16 | # print(np.diag( np.minimum( delta, np.maximum(e, epsilon) ) )) 17 | Q_PSD = V.dot(np.diag( np.minimum( delta, np.maximum(e, epsilon) ) )).dot(V.T) 18 | return Q_PSD 19 | 20 | def gradients(Xs, Xu, Y, S, U, B, Bcon): 21 | Sinv = np.linalg.inv(S) 22 | R = Sinv.dot(U).dot(B).dot(S) 23 | # R = np.linalg.multi_dot([Sinv, U, B, S, X]) 24 | Error = Y - Bcon.dot(Xu)- R.dot(Xs) 25 | e = np.linalg.norm(Error, 'fro') 26 | 27 | if len(locals()) >= 2: 28 | temp1 = Sinv.T .dot(-Error).dot(Xs.T) 29 | S_grad = -temp1.dot(R.T) + B.T.dot(U.T).dot(temp1) 30 | U_grad = temp1.dot(S.T).dot(B.T) 31 | B_grad = - U.T.dot(-temp1).dot(S.T) 32 | Bcon_grad = - Error.dot(Xu.T) 33 | return e, S_grad, U_grad, B_grad, Bcon_grad 34 | else: 35 | return e 36 | 37 | def checkdstable(A): 38 | n = len(A) 39 | P = solve_discrete_lyapunov(A.T, np.identity(n)) 40 | S = sqrtm(P) 41 | invS = np.linalg.inv(S) 42 | UB = S.dot(A).dot(invS) 43 | [U,B] = polar(UB) 44 | B = projectPSD(B,0,1) 45 | return P,S,U,B 46 | 47 | def stabilize_discrete(Xs, Xu, Y, S = None, U = None, B = None, Bcon = None): 48 | n = len(Xs) # number of Koopman basis functions 49 | # na2 = np.linalg.norm(Y, 'fro')**2 50 | na2 = np.linalg.norm(Y, 'fro') 51 | X = np.vstack((Xs, Xu)) 52 | Nx = np.ma.size(Xs,0) # number of rows 53 | Nu = np.ma.size(Xu,0) 54 | 55 | print(S) 56 | if S is None: 57 | # Initialization of S, U, and B 58 | S = np.identity(n) 59 | temp = Y.dot(pinv2(X)) 60 | [U, B] = polar(temp[:Nx,:Nx]) 61 | B = projectPSD(B, 0, 1) 62 | Bcon = temp[:Nx, Nx:] 63 | 64 | # parameters 65 | alpha0 = 0.5 # parameter of FGM 66 | lsparam = 1.5 # parameter; has to be larger than 1 for convergence 67 | lsitermax = 20 68 | gradient = 0 # 1 for standard Gradient Descent; 0 for FGM 69 | print(S) 70 | if np.linalg.cond(S) > 1e12 : 71 | print(" Initial S is ill-conditioned") 72 | 73 | # initial step length: 1/L 74 | eS,_ = np.linalg.eig(S) 75 | L = (np.max(eS)/ np.min(eS))**2 76 | 77 | # Initialization 78 | error,_,_,_,_ = gradients(Xs,Xu,Y,S,U,B,Bcon) 79 | print("Error is ", error) 80 | step = 1/L 81 | i = 1 82 | alpha0 = 0.5 83 | alpha = alpha0 84 | Ys = S 85 | Yu = U 86 | Yb = B 87 | Yb_con = Bcon 88 | restarti = 1 89 | 90 | max_iter = 30 91 | while i < max_iter: 92 | # compute gradient 93 | 94 | _, gS, gU, gB, gB_con = gradients(Xs,Xu, Y,S,U,B, Bcon) 95 | error_next = math.inf 96 | inner_iter = 1 97 | step = step * 2 98 | 99 | # print("This is error", error, " at iteration: ", i) 100 | # print("error: ", error) 101 | # Line Search 102 | while ( (error_next > error) and ( ((i == 1) and (inner_iter <= 100)) or (inner_iter <= lsitermax) ) ): 103 | Sn = Ys - gS*step 104 | Un = Yu - gU*step 105 | Bn = Yb - gB*step 106 | Bn_con = Yb_con - gB_con * step 107 | 108 | # Project onto feasible set 109 | Sn = projectPSD(Sn, 1e-15) 110 | Un,_ = polar(Un) 111 | Bn = projectPSD(Bn, 0, 1) 112 | # print("Projected") 113 | # print(Sn) 114 | error_next,_,_,_,_ = gradients(Xs,Xu, Y, Sn, Un, Bn, Bn_con) 115 | step = step / lsparam 116 | inner_iter = inner_iter + 1 117 | # print(inner_iter) 118 | if (i == 1): 119 | inner_iter0 = inner_iter 120 | 121 | # Conjugate with FGM weights, if cost decreased; else, restart FGM 122 | alpha_next = (math.sqrt(alpha**4 + 4*alpha**2) - alpha**2 )/2 123 | beta = alpha * (1 - alpha) / (alpha**2 + alpha_next) 124 | 125 | if (inner_iter >= lsitermax + 1): # line search failed 126 | if restarti == 1: 127 | # Restart FGM if not a descent direction 128 | restarti = 0 129 | alpha_next = alpha0 130 | Ys = S 131 | Yu = U 132 | Yb = B 133 | Yb_con = Bcon 134 | error_next = error 135 | print(" No descent: Restart FGM") 136 | 137 | # Reinitialize step length 138 | eS,_ = np.linalg.eig(S) 139 | L = (np.max(eS)/ np.min(eS))**2 140 | # Use information from the first step: how many steps to decrease 141 | step = 1/L/lsparam**inner_iter0 142 | elif (restarti == 0): # no previous restart/descent direction 143 | error_next = error 144 | break 145 | else: 146 | restarti = 1 147 | if (gradient == 1): 148 | beta = 0 149 | Ys = Sn + beta * (Sn - S) 150 | Yu = Un + beta * (Un - U) 151 | Yb = Bn + beta * (Bn - B) 152 | Yb_con = Bn_con + beta * (Bn_con - Bcon) 153 | # Keep new iterates in memory 154 | S = Sn 155 | U = Un 156 | B = Bn 157 | Bcon = Bn_con 158 | i = i + 1 159 | error = error_next 160 | alpha = alpha_next 161 | 162 | # Check if error is small (1e-6 relative error) 163 | if (error < 1e-12*na2): 164 | print("The algorithm converged") 165 | break 166 | Kd = np.linalg.inv(S).dot(U).dot(B).dot(S) 167 | return Kd, S, U, B, Bcon, error 168 | -------------------------------------------------------------------------------- /autompc/benchmarks/cartpole.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-01-09 2 | 3 | # Standard library includes 4 | import sys 5 | 6 | # External library includes 7 | import numpy as np 8 | import matplotlib.animation as animation 9 | 10 | # Project includes 11 | from .benchmark import Benchmark 12 | from ..utils.data_generation import * 13 | from .. import System 14 | from ..tasks import Task 15 | from ..costs import ThresholdCost 16 | 17 | def cartpole_simp_dynamics(y, u, g = 9.8, m = 1, L = 1, b = 0.1): 18 | """ 19 | Parameters 20 | ---------- 21 | y : states 22 | u : control 23 | 24 | Returns 25 | ------- 26 | A list describing the dynamics of the cart cart pole 27 | """ 28 | theta, omega, x, dx = y 29 | return np.array([omega, 30 | g * np.sin(theta)/L - b * omega / (m*L**2) + u * np.cos(theta)/L, 31 | dx, 32 | u]) 33 | 34 | def dt_cartpole_dynamics(y,u,dt,g=9.8,m=1,L=1,b=1.0): 35 | y += dt * cartpole_simp_dynamics(y,u[0],g,m,L,b) 36 | return y 37 | 38 | class CartpoleSwingupBenchmark(Benchmark): 39 | """ 40 | This benchmark uses the cartpole system and is consistent with the 41 | experiments in the ICRA 2021 paper. The task is to move the pole 42 | from the down position to the up position. The performance metric 43 | returns 1 for every observation which is more than 0.2 away from the goal 44 | in either the angle or angular velocity dimensions, and 0 otherwise. 45 | """ 46 | def __init__(self, data_gen_method="uniform_random"): 47 | name = "cartpole_swingup" 48 | system = ampc.System(["theta", "omega", "x", "dx"], ["u"]) 49 | system.dt = 0.05 50 | 51 | cost = ThresholdCost(system, goal=np.zeros(4), threshold=0.2, obs_range=(0,3)) 52 | task = Task(system) 53 | task.set_cost(cost) 54 | task.set_ctrl_bound("u", -20.0, 20.0) 55 | init_obs = np.array([3.1, 0.0, 0.0, 0.0]) 56 | task.set_init_obs(init_obs) 57 | task.set_num_steps(200) 58 | 59 | super().__init__(name, system, task, data_gen_method) 60 | 61 | def dynamics(self, x, u): 62 | return dt_cartpole_dynamics(x,u,self.system.dt,g=9.8,m=1,L=1,b=1.0) 63 | 64 | def visualize(self, fig, ax, traj, margin=5.0): 65 | """ 66 | Visualize the cartpole trajectory. 67 | 68 | Parameters 69 | ---------- 70 | fig : matplotlib.figure.Figure 71 | Figure to generate visualization in. 72 | 73 | ax : matplotlib.axes.Axes 74 | Axes to create visualization in. 75 | 76 | traj : Trajectory 77 | Trajectory to visualize 78 | 79 | margin : float 80 | Shift the viewing window by this amount when the 81 | cartpole reaches the edge of the screen 82 | """ 83 | ax.plot([-10000, 10000.0], [0.0, 0.0], "k-", lw=1) 84 | ax.set_xlim([-10.0, 10.0]) 85 | ax.set_ylim([-2.0, 2.0]) 86 | ax.set_aspect("equal") 87 | dt = self.system.dt 88 | 89 | line, = ax.plot([0.0, 0.0], [0.0, -1.0], 'o-', lw=2) 90 | time_text = ax.text(0.02, 0.85, '', transform=ax.transAxes) 91 | ctrl_text = ax.text(0.7, 0.85, '', transform=ax.transAxes) 92 | 93 | def init(): 94 | line.set_data([0.0, 0.0], [0.0, -1.0]) 95 | time_text.set_text('') 96 | return line, time_text 97 | 98 | nframes = traj.size + 50 99 | def animate(i): 100 | i %= nframes 101 | i = min(i, traj.size-1) 102 | if i == 0: 103 | ax.set_xlim([-10.0, 10.0]) 104 | #i = min(i, ts.shape[0]) 105 | line.set_data([traj[i,"x"], traj[i,"x"]+np.sin(traj[i,"theta"]+np.pi)], 106 | [0, -np.cos(traj[i,"theta"] + np.pi)]) 107 | time_text.set_text('t={:.2f}'.format(dt*i)) 108 | ctrl_text.set_text("u={:.2f}".format(traj[i,"u"])) 109 | xmin, xmax = ax.get_xlim() 110 | if traj[i, "x"] < xmin: 111 | ax.set_xlim([traj[i,"x"] - margin, traj[i,"x"] + 20.0 - margin]) 112 | if traj[i, "x"] > xmax: 113 | ax.set_xlim([traj[i,"x"] - 20.0 + margin, traj[i,"x"] + margin]) 114 | return line, time_text 115 | 116 | anim = animation.FuncAnimation(fig, animate, frames=6*nframes, interval=dt*1000.0, 117 | blit=False, init_func=init) 118 | 119 | return anim 120 | 121 | def _gen_trajs(self, n_trajs, traj_len, rng): 122 | init_min = np.array([-1.0, 0.0, 0.0, 0.0]) 123 | init_max = np.array([1.0, 0.0, 0.0, 0.0]) 124 | if self._data_gen_method == "uniform_random": 125 | return uniform_random_generate(self.system, self.task, self.dynamics, rng, 126 | init_min=init_min, init_max=init_max, 127 | traj_len=traj_len, n_trajs=n_trajs) 128 | elif self._data_gen_method == "periodic_control": 129 | return periodic_control_generate(self.system, self.task, self.dynamics, rng, 130 | init_min=init_min, init_max=init_max, U_1=np.ones(1), 131 | traj_len=traj_len, n_trajs=n_trajs) 132 | elif self._data_gen_method == "multisine": 133 | return multisine_generate(self.system, self.task, self.dynamics, rng, 134 | init_min=init_min, init_max=init_max, n_freqs=20, 135 | traj_len=traj_len, n_trajs=n_trajs) 136 | elif self._data_gen_method == "random_walk": 137 | return random_walk_generate(self.system, self.task, self.dynamics, rng, 138 | init_min=init_min, init_max=init_max, walk_rate=1.0, 139 | traj_len=traj_len, n_trajs=n_trajs) 140 | 141 | def gen_trajs(self, seed, n_trajs, traj_len=200): 142 | rng = np.random.default_rng(seed) 143 | return self._gen_trajs(n_trajs, traj_len, rng) 144 | 145 | 146 | @staticmethod 147 | def data_gen_methods(): 148 | return ["uniform_random", "periodic_control", "multisine", "random_walk"] 149 | -------------------------------------------------------------------------------- /autompc/sysid/arx.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | from pdb import set_trace 4 | 5 | import numpy as np 6 | import numpy.linalg as la 7 | 8 | from ConfigSpace import ConfigurationSpace 9 | from ConfigSpace.hyperparameters import UniformIntegerHyperparameter 10 | 11 | from .model import Model, ModelFactory 12 | #from ..hyper import IntRangeHyperparam 13 | 14 | class ARXFactory(ModelFactory): 15 | R""" 16 | Autoregression with Exogenous Variable (ARX) learns the dynamics as 17 | a linear function of the last :math:`k` observations and controls. 18 | That is 19 | 20 | .. math:: 21 | x_{t+1} = [x_t, \ldots x_{t-k+1}, u_t, \ldots, u_{t-k+1}] \theta 22 | 23 | The model is trained least-squared linear regression. 24 | 25 | Hyperparameters: 26 | 27 | - *history* (Type: int, Low: 1, High: 10, Default: 4): Size of history window 28 | for ARX model. 29 | """ 30 | def __init__(self, *args, **kwargs): 31 | super().__init__(*args, **kwargs) 32 | self.Model = ARX 33 | self.name = "ARX" 34 | 35 | def get_configuration_space(self): 36 | cs = ConfigurationSpace() 37 | history = UniformIntegerHyperparameter(name='history', 38 | lower=1, upper=10, default_value=4) 39 | cs.add_hyperparameter(history) 40 | return cs 41 | 42 | class ARX(Model): 43 | def __init__(self, system, history): 44 | super().__init__(system) 45 | self.k = history 46 | 47 | def _get_feature_vector(self, traj, t=None): 48 | k = self.k 49 | if t is None: 50 | t = len(traj) 51 | 52 | feature_elements = [traj[t-1].obs] 53 | for i in range(t-2, t-k-1, -1): 54 | if i >= 0: 55 | feature_elements += [traj[i].obs, traj[i].ctrl] 56 | else: 57 | feature_elements += [traj[0].obs, traj[0].ctrl] 58 | feature_elements += [np.ones(1), traj[t-1].ctrl] 59 | return np.concatenate(feature_elements) 60 | 61 | def _get_all_feature_vectors(self, traj): 62 | k = self.k 63 | feature_vectors = np.zeros((len(traj), k*(self.system.obs_dim + self.system.ctrl_dim)+1)) 64 | feature_vectors[:,:self.system.obs_dim] = traj.obs 65 | j = self.system.obs_dim 66 | for i in range(1, k, 1): 67 | feature_vectors[:,j:j+self.system.obs_dim] = np.concatenate([traj.obs[:1,:]]*i + [traj.obs[:-i, :]]) 68 | j += self.system.obs_dim 69 | feature_vectors[:,j:j+self.system.ctrl_dim] = np.concatenate([traj.ctrls[:1,:]]*i + [traj.ctrls[:-i, :]]) 70 | j += self.system.ctrl_dim 71 | feature_vectors[:,-(self.system.ctrl_dim+1)] = 1 72 | feature_vectors[:,-self.system.ctrl_dim:] = traj.ctrls 73 | 74 | return feature_vectors 75 | 76 | def _get_fvec_size(self): 77 | k = self.k 78 | return 1 + k*self.system.obs_dim + k*self.system.ctrl_dim 79 | 80 | def _get_training_matrix_and_targets(self, trajs): 81 | nsamples = sum([len(traj) for traj in trajs]) 82 | matrix = np.zeros((nsamples, self._get_fvec_size())) 83 | targets = np.zeros((nsamples, self.system.obs_dim)) 84 | 85 | i = 0 86 | for traj in trajs: 87 | for t in range(1, len(traj)): 88 | matrix[i, :] = self._get_feature_vector(traj, t) 89 | targets[i, :] = traj[t].obs 90 | i += 1 91 | 92 | return matrix, targets 93 | 94 | def update_state(self, state, new_ctrl, new_obs): 95 | # Shift the targets 96 | newstate = self.A @ state + self.B @ new_ctrl 97 | newstate[:self.system.obs_dim] = new_obs 98 | 99 | return newstate 100 | 101 | def traj_to_state(self, traj): 102 | return self._get_feature_vector(traj)[:-self.system.ctrl_dim] 103 | 104 | def traj_to_states(self, traj): 105 | return self._get_all_feature_vectors(traj)[:, :-self.system.ctrl_dim] 106 | 107 | def state_to_obs(self, state): 108 | return state[0:self.system.obs_dim] 109 | 110 | def train(self, trajs, silent=False): 111 | matrix, targets = self._get_training_matrix_and_targets(trajs) 112 | 113 | coeffs = np.zeros((self.system.obs_dim, self._get_fvec_size())) 114 | for i in range(targets.shape[1]): 115 | res, _, _, _ = la.lstsq(matrix, targets[:,i], rcond=None) 116 | coeffs[i,:] = res 117 | 118 | # First we construct the system matrices 119 | A = np.zeros((self.state_dim, self.state_dim)) 120 | B = np.zeros((self.state_dim, self.system.ctrl_dim)) 121 | 122 | # Constant term 123 | A[-1,-1] = 1.0 124 | 125 | # Shift history 126 | n = self.system.obs_dim 127 | l = self.system.ctrl_dim 128 | m = self.system.obs_dim + self.system.ctrl_dim 129 | k = self.k 130 | 131 | if k > 1: 132 | A[n : 2*n, 0 : n] = np.eye(n) 133 | for i in range(k-2): 134 | A[(i+1)*m+n : (i+2)*m+n, i*m+n : (i+1)*m+n] = np.eye(m) 135 | 136 | # Predict new observation 137 | A[0 : n, :] = coeffs[:, :-l] 138 | 139 | # Add new control 140 | B[0 : n, :] = coeffs[:, -l:] 141 | B[2*n : 2*n + l, :] = np.eye(l) 142 | 143 | self.A, self.B = A, B 144 | 145 | 146 | def pred(self, state, ctrl): 147 | statenew = self.A @ state + self.B @ ctrl 148 | 149 | return statenew 150 | 151 | def pred_batch(self, states, ctrls): 152 | statesnew = self.A @ states.T + self.B @ ctrls.T 153 | 154 | return statesnew.T 155 | 156 | def pred_diff(self, state, ctrl): 157 | statenew = self.A @ state + self.B @ ctrl 158 | 159 | return statenew, self.A, self.B 160 | 161 | def to_linear(self): 162 | return self.A, self.B 163 | 164 | @property 165 | def state_dim(self): 166 | return self._get_fvec_size() - self.system.ctrl_dim 167 | 168 | 169 | def get_parameters(self): 170 | return {"coeffs" : np.copy(self.coeffs)} 171 | 172 | def set_parameters(self, params): 173 | self.coeffs = np.copy(params["coeffs"]) 174 | 175 | 176 | -------------------------------------------------------------------------------- /examples/1_Basics.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Example 1: Basics" 8 | ] 9 | }, 10 | { 11 | "cell_type": "markdown", 12 | "metadata": {}, 13 | "source": [ 14 | "Begin by importing AutoMPC." 15 | ] 16 | }, 17 | { 18 | "cell_type": "code", 19 | "execution_count": 1, 20 | "metadata": {}, 21 | "outputs": [ 22 | { 23 | "name": "stdout", 24 | "output_type": "stream", 25 | "text": [ 26 | "Loading AutoMPC...\n", 27 | "Finished loading AutoMPC\n" 28 | ] 29 | } 30 | ], 31 | "source": [ 32 | "import autompc as ampc\n", 33 | "import numpy as np" 34 | ] 35 | }, 36 | { 37 | "cell_type": "markdown", 38 | "metadata": {}, 39 | "source": [ 40 | "## Systems\n", 41 | "Let's begin by showing how to define a System. In AutoMPC, a System defines the variables of control and observation for a particular robot. Here we define `simple_sys` which has to observation variables (x and y) and one control variable (u). Optionally, the system can also include the time step at which is data is sampled for the system. Here we define the time step as 0.05 s." 42 | ] 43 | }, 44 | { 45 | "cell_type": "code", 46 | "execution_count": 2, 47 | "metadata": {}, 48 | "outputs": [], 49 | "source": [ 50 | "simple_sys = ampc.System([\"x\", \"y\"], [\"u\"], dt=0.05)" 51 | ] 52 | }, 53 | { 54 | "cell_type": "markdown", 55 | "metadata": {}, 56 | "source": [ 57 | "Given a system, we can access its properties as follows" 58 | ] 59 | }, 60 | { 61 | "cell_type": "code", 62 | "execution_count": 3, 63 | "metadata": {}, 64 | "outputs": [ 65 | { 66 | "name": "stdout", 67 | "output_type": "stream", 68 | "text": [ 69 | "Observation Dimension: 2\n", 70 | "Observation Variables: ['x', 'y']\n", 71 | "Control Dimension: 1\n", 72 | "Control Variables: ['u']\n" 73 | ] 74 | } 75 | ], 76 | "source": [ 77 | "print(\"Observation Dimension: \", simple_sys.obs_dim)\n", 78 | "print(\"Observation Variables: \", simple_sys.observations)\n", 79 | "\n", 80 | "print(\"Control Dimension: \", simple_sys.ctrl_dim)\n", 81 | "print(\"Control Variables: \", simple_sys.controls)" 82 | ] 83 | }, 84 | { 85 | "cell_type": "markdown", 86 | "metadata": {}, 87 | "source": [ 88 | "## Trajectories\n", 89 | "The Trajectory class stores a sequence of controls and observations. Trajectories are defined with respect to a particular system.\n", 90 | "\n", 91 | "Here we define a zero trajectory for `simple_sys` with 10 time steps." 92 | ] 93 | }, 94 | { 95 | "cell_type": "code", 96 | "execution_count": 4, 97 | "metadata": {}, 98 | "outputs": [], 99 | "source": [ 100 | "traj = ampc.zeros(simple_sys, 10)" 101 | ] 102 | }, 103 | { 104 | "cell_type": "markdown", 105 | "metadata": {}, 106 | "source": [ 107 | "There are a couple different ways to set trajectory values. We demonstrate a few below:" 108 | ] 109 | }, 110 | { 111 | "cell_type": "code", 112 | "execution_count": 5, 113 | "metadata": {}, 114 | "outputs": [], 115 | "source": [ 116 | "traj[0, \"x\"] = 1.0 # Set x to 1 at timestep 0\n", 117 | "traj[1, \"u\"] = 2.0 # Set u to 2 at timestep 1\n", 118 | "traj[2].obs[:] = np.array([3.0, 4.0]) # Set the observation (x and y) to [3,4] at timestep 2\n", 119 | "traj[3].ctrl[:] = np.array([5.0]) # Set the control (u) to [5] at timestep 3" 120 | ] 121 | }, 122 | { 123 | "cell_type": "markdown", 124 | "metadata": {}, 125 | "source": [ 126 | "Similarly, there are a number of reading trajectory values." 127 | ] 128 | }, 129 | { 130 | "cell_type": "code", 131 | "execution_count": 6, 132 | "metadata": {}, 133 | "outputs": [ 134 | { 135 | "name": "stdout", 136 | "output_type": "stream", 137 | "text": [ 138 | "Value of y at timestep 2: 4.0\n", 139 | "Observation at timestep 0: [1. 0.]\n", 140 | "Control at timestep 1: [2.]\n" 141 | ] 142 | } 143 | ], 144 | "source": [ 145 | "print(\"Value of y at timestep 2: \", traj[2, \"y\"])\n", 146 | "print(\"Observation at timestep 0: \", traj[0].obs)\n", 147 | "print(\"Control at timestep 1: \", traj[1].ctrl)" 148 | ] 149 | }, 150 | { 151 | "cell_type": "markdown", 152 | "metadata": {}, 153 | "source": [ 154 | "We can also access the entire set of observations and controls for a trajectory as numpy arrays:" 155 | ] 156 | }, 157 | { 158 | "cell_type": "code", 159 | "execution_count": 7, 160 | "metadata": {}, 161 | "outputs": [ 162 | { 163 | "name": "stdout", 164 | "output_type": "stream", 165 | "text": [ 166 | "Observations\n", 167 | "------------\n", 168 | "[[1. 0.]\n", 169 | " [0. 0.]\n", 170 | " [3. 4.]\n", 171 | " [0. 0.]\n", 172 | " [0. 0.]\n", 173 | " [0. 0.]\n", 174 | " [0. 0.]\n", 175 | " [0. 0.]\n", 176 | " [0. 0.]\n", 177 | " [0. 0.]]\n", 178 | "\n", 179 | "Controls\n", 180 | "--------\n", 181 | "[[0.]\n", 182 | " [2.]\n", 183 | " [0.]\n", 184 | " [5.]\n", 185 | " [0.]\n", 186 | " [0.]\n", 187 | " [0.]\n", 188 | " [0.]\n", 189 | " [0.]\n", 190 | " [0.]]\n" 191 | ] 192 | } 193 | ], 194 | "source": [ 195 | "print(\"Observations\")\n", 196 | "print(\"------------\")\n", 197 | "print(traj.obs)\n", 198 | "\n", 199 | "print(\"\")\n", 200 | "print(\"Controls\")\n", 201 | "print(\"--------\")\n", 202 | "print(traj.ctrls)" 203 | ] 204 | } 205 | ], 206 | "metadata": { 207 | "kernelspec": { 208 | "display_name": "Python 3", 209 | "language": "python", 210 | "name": "python3" 211 | }, 212 | "language_info": { 213 | "codemirror_mode": { 214 | "name": "ipython", 215 | "version": 3 216 | }, 217 | "file_extension": ".py", 218 | "mimetype": "text/x-python", 219 | "name": "python", 220 | "nbconvert_exporter": "python", 221 | "pygments_lexer": "ipython3", 222 | "version": "3.8.7" 223 | } 224 | }, 225 | "nbformat": 4, 226 | "nbformat_minor": 4 227 | } 228 | -------------------------------------------------------------------------------- /autompc/benchmarks/cartpole_v2.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-01-09 2 | 3 | # Standard library includes 4 | import sys, os 5 | import pickle 6 | 7 | # External library includes 8 | import numpy as np 9 | import matplotlib.animation as animation 10 | 11 | # Project includes 12 | from .benchmark import Benchmark 13 | from ..utils.data_generation import * 14 | from .. import System 15 | from ..tasks import Task 16 | from ..costs import BoxThresholdCost,ThresholdCost 17 | 18 | def cartpole_simp_dynamics(y, u, g = 9.8, m = 1, L = 1, b = 0.1): 19 | """ 20 | Parameters 21 | ---------- 22 | y : states 23 | u : control 24 | 25 | Returns 26 | ------- 27 | A list describing the dynamics of the cart cart pole 28 | """ 29 | theta, omega, x, dx = y 30 | return np.array([omega, 31 | g * np.sin(theta)/L - b * omega / (m*L**2) + u * np.cos(theta)/L, 32 | dx, 33 | u]) 34 | 35 | def dt_cartpole_dynamics(y,u,dt,g=9.8,m=1,L=1,b=1.0): 36 | y += dt * cartpole_simp_dynamics(y,u[0],g,m,L,b) 37 | return y 38 | 39 | class CartpoleSwingupV2Benchmark(Benchmark): 40 | """ 41 | This benchmark uses the cartpole system and differs from CartpoleSwingupBenchmark 42 | in that the performance metric requires the cartpole to stay within the [-10, 10] 43 | range. 44 | """ 45 | def __init__(self, data_gen_method="uniform_random"): 46 | name = "cartpole_swingup" 47 | system = ampc.System(["theta", "omega", "x", "dx"], ["u"]) 48 | system.dt = 0.05 49 | 50 | limits = np.array([[-0.2, 0.2], [-0.2, 0.2], [-10.0, 10.0], [-np.inf, np.inf]]) 51 | cost = BoxThresholdCost(system, limits, goal=np.zeros(4)) 52 | task = Task(system) 53 | task.set_cost(cost) 54 | task.set_ctrl_bound("u", -20.0, 20.0) 55 | init_obs = np.array([3.1, 0.0, 0.0, 0.0]) 56 | task.set_init_obs(init_obs) 57 | task.set_num_steps(200) 58 | 59 | super().__init__(name, system, task, data_gen_method) 60 | 61 | def dynamics(self, x, u): 62 | return dt_cartpole_dynamics(x,u,self.system.dt,g=0.8,m=1,L=1,b=1.0) 63 | 64 | def visualize(self, fig, ax, traj, margin=5.0): 65 | """ 66 | Visualize the cartpole trajectory. 67 | 68 | Parameters 69 | ---------- 70 | fig : matplotlib.figure.Figure 71 | Figure to generate visualization in. 72 | 73 | ax : matplotlib.axes.Axes 74 | Axes to create visualization in. 75 | 76 | traj : Trajectory 77 | Trajectory to visualize 78 | 79 | margin : float 80 | Shift the viewing window by this amount when the 81 | cartpole reaches the edge of the screen 82 | """ 83 | 84 | ax.plot([-10000, 10000.0], [0.0, 0.0], "k-", lw=1) 85 | ax.set_xlim([-10.0, 10.0]) 86 | ax.set_ylim([-2.0, 2.0]) 87 | ax.set_aspect("equal") 88 | dt = self.system.dt 89 | 90 | line, = ax.plot([0.0, 0.0], [0.0, -1.0], 'o-', lw=2) 91 | time_text = ax.text(0.02, 0.85, '', transform=ax.transAxes) 92 | ctrl_text = ax.text(0.7, 0.85, '', transform=ax.transAxes) 93 | 94 | def init(): 95 | line.set_data([0.0, 0.0], [0.0, -1.0]) 96 | time_text.set_text('') 97 | return line, time_text 98 | 99 | nframes = traj.size + 50 100 | def animate(i): 101 | i %= nframes 102 | i = min(i, traj.size-1) 103 | if i == 0: 104 | ax.set_xlim([-10.0, 10.0]) 105 | #i = min(i, ts.shape[0]) 106 | line.set_data([traj[i,"x"], traj[i,"x"]+np.sin(traj[i,"theta"]+np.pi)], 107 | [0, -np.cos(traj[i,"theta"] + np.pi)]) 108 | time_text.set_text('t={:.2f}'.format(dt*i)) 109 | ctrl_text.set_text("u={:.2f}".format(traj[i,"u"])) 110 | xmin, xmax = ax.get_xlim() 111 | if traj[i, "x"] < xmin: 112 | ax.set_xlim([traj[i,"x"] - margin, traj[i,"x"] + 20.0 - margin]) 113 | if traj[i, "x"] > xmax: 114 | ax.set_xlim([traj[i,"x"] - 20.0 + margin, traj[i,"x"] + margin]) 115 | return line, time_text 116 | 117 | anim = animation.FuncAnimation(fig, animate, frames=6*nframes, interval=dt*1000.0, 118 | blit=False, init_func=init) 119 | 120 | return anim 121 | 122 | def _gen_trajs(self, n_trajs, traj_len, rng): 123 | init_min = np.array([-1.0, 0.0, 0.0, 0.0]) 124 | init_max = np.array([1.0, 0.0, 0.0, 0.0]) 125 | if self._data_gen_method == "uniform_random": 126 | return uniform_random_generate(self.system, self.task, self.dynamics, rng, 127 | init_min=init_min, init_max=init_max, 128 | traj_len=traj_len, n_trajs=n_trajs) 129 | elif self._data_gen_method == "periodic_control": 130 | return periodic_control_generate(self.system, self.task, self.dynamics, rng, 131 | init_min=init_min, init_max=init_max, U_1=np.ones(1), 132 | traj_len=traj_len, n_trajs=n_trajs) 133 | elif self._data_gen_method == "multisine": 134 | return multisine_generate(self.system, self.task, self.dynamics, rng, 135 | init_min=init_min, init_max=init_max, n_freqs=20, 136 | traj_len=traj_len, n_trajs=n_trajs) 137 | elif self._data_gen_method == "random_walk": 138 | return random_walk_generate(self.system, self.task, self.dynamics, rng, 139 | init_min=init_min, init_max=init_max, walk_rate=1.0, 140 | traj_len=traj_len, n_trajs=n_trajs) 141 | 142 | def gen_trajs(self, seed, n_trajs, traj_len=200): 143 | rng = np.random.default_rng(seed) 144 | return self._gen_trajs(n_trajs, traj_len, rng) 145 | 146 | def get_cached_tune_result(self): 147 | dirname = os.path.dirname(__file__) 148 | pklname = os.path.join(dirname, 149 | "../../assets/cached_tunes/cartpole_tune_result.pkl") 150 | with open(pklname, "rb") as f: 151 | tune_result = pickle.load(f) 152 | 153 | return tune_result 154 | 155 | 156 | @staticmethod 157 | def data_gen_methods(): 158 | return ["uniform_random", "periodic_control", "multisine", "random_walk"] 159 | -------------------------------------------------------------------------------- /autompc/tuning/model_tuner.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | from collections import namedtuple 4 | 5 | from smac.scenario.scenario import Scenario 6 | from smac.facade.smac_hpo_facade import SMAC4HPO 7 | 8 | import ConfigSpace as CS 9 | import ConfigSpace.hyperparameters as CSH 10 | import ConfigSpace.conditions as CSC 11 | import copy 12 | 13 | from ..utils.cs_utils import add_configuration_space 14 | 15 | from ConfigSpace import ConfigurationSpace 16 | from ConfigSpace.hyperparameters import ( 17 | Hyperparameter, 18 | Constant, 19 | FloatHyperparameter, 20 | ) 21 | from ConfigSpace.conditions import ( 22 | ConditionComponent, 23 | AbstractCondition, 24 | AbstractConjunction, 25 | EqualsCondition, 26 | ) 27 | from ConfigSpace.forbidden import ( 28 | AbstractForbiddenComponent, 29 | AbstractForbiddenClause, 30 | AbstractForbiddenConjunction, 31 | ) 32 | 33 | import numpy as np 34 | 35 | from pdb import set_trace 36 | 37 | ModelTuneResult = namedtuple("ModelTuneResult", ["inc_cfg", "cfgs", 38 | "inc_cfgs", "costs", "inc_costs"]) 39 | """ 40 | The ModelTuneResult contains information about a tuning process. 41 | 42 | .. py:attribute:: inc_cfg 43 | 44 | The final tuned configuration. 45 | 46 | .. py:attribute:: cfgs 47 | 48 | List of configurations. The configuration evaluated at each 49 | tuning iteration. 50 | 51 | .. py:attribute:: costs 52 | 53 | The cost (evaluation score) observed at each iteration of 54 | tuning iteration. 55 | 56 | .. py:attribute:: inc_cfgs 57 | 58 | The incumbent (best found so far) configuration at each 59 | tuning iteration. 60 | 61 | .. py:attribute:: inc_costs 62 | 63 | The incumbent score at each tuning iteration. 64 | """ 65 | 66 | class ModelTuner: 67 | """ 68 | The ModelTuner class is used for tuning system ID models based 69 | on prediction accuracy. 70 | """ 71 | def __init__(self, system, evaluator): 72 | """ 73 | Parameters 74 | ---------- 75 | system : System 76 | System for which models will be tuned 77 | evaluator : ModelEvaluator 78 | This evaluator object will be used to asses model 79 | configurations 80 | """ 81 | self.system = system 82 | self.evaluator = evaluator 83 | self.model_factories = [] 84 | 85 | def add_model_factory(self, model_factory, cs=None): 86 | """ 87 | Add a model factory which is an option for tuning. 88 | Multiple model factories can be added and the tuner 89 | will select between them. 90 | 91 | Parameters 92 | ---------- 93 | model_factory : ModelFactory 94 | 95 | cs : ConfigurationSpace 96 | Configuration space for model factory. This only needs to be 97 | passed if the configuration space is customized, otherwise 98 | it will be derived from the model_factory. 99 | """ 100 | if cs is None: 101 | cs = model_factory.get_configuration_space() 102 | self.model_factories.append((model_factory, cs)) 103 | 104 | def _get_model_cfg(self, cfg_combined): 105 | for model_factory, cs in self.model_factories: 106 | if model_factory.name != cfg_combined["model"]: 107 | continue 108 | cfg = cs.get_default_configuration() 109 | prefix = "_" + model_factory.name + ":" 110 | for key, val in cfg_combined.get_dictionary().items(): 111 | if key[:len(prefix)] == prefix: 112 | cfg[key.split(":")[1]] = val 113 | return model_factory, cfg 114 | 115 | def _evaluate(self, cfg_combined): 116 | print("Evaluating Cfg:") 117 | print(cfg_combined) 118 | model_factory, cfg = self._get_model_cfg(cfg_combined) 119 | value = self.evaluator(model_factory, cfg) 120 | print("Model Score ", value) 121 | return value 122 | 123 | 124 | 125 | def run(self, rng, n_iters=10): 126 | """ 127 | Run tuning process 128 | 129 | Parameters 130 | ---------- 131 | rng : numpy.random.Generator 132 | Random number generator used for tuning 133 | 134 | n_iters : int 135 | Number of tuning iterations to run 136 | 137 | Returns 138 | ------- 139 | model : Model 140 | Resulting model 141 | 142 | tune_result : ModelTuneResult 143 | Additional information from tuning process 144 | """ 145 | # Construct configuration space 146 | cs_combined = CS.ConfigurationSpace() 147 | 148 | model_choice = CSH.CategoricalHyperparameter("model", 149 | choices=[model_factory.name for model_factory, _ 150 | in self.model_factories]) 151 | cs_combined.add_hyperparameter(model_choice) 152 | for model_factory, cs in self.model_factories: 153 | model_name = model_factory.name 154 | add_configuration_space(cs_combined, "_" + model_name, 155 | cs, parent_hyperparameter={"parent" : model_choice, 156 | "value" : model_name}) 157 | 158 | smac_rng = np.random.RandomState(rng.integers(1 << 31)) 159 | scenario = Scenario({"run_obj": "quality", 160 | "runcount-limit": n_iters, 161 | "cs": cs_combined, 162 | "deterministic": "true", 163 | "limit_resources" : False 164 | }) 165 | 166 | smac = SMAC4HPO(scenario=scenario, rng=smac_rng, 167 | tae_runner=self._evaluate) 168 | 169 | incumbent = smac.optimize() 170 | 171 | ret_value = dict() 172 | inc_cost = float("inf") 173 | inc_costs = [] 174 | evaluated_costs = [] 175 | evaluated_cfgs = [] 176 | inc_cfgs = [] 177 | costs_and_config_ids = [] 178 | inc_cfg = None 179 | for key, val in smac.runhistory.data.items(): 180 | cfg = smac.runhistory.ids_config[key.config_id] 181 | if val.cost < inc_cost: 182 | inc_cost = val.cost 183 | inc_cfg = cfg 184 | inc_costs.append(inc_cost) 185 | evaluated_costs.append(val.cost) 186 | evaluated_cfgs.append(cfg) 187 | inc_cfgs.append(inc_cfg) 188 | 189 | tune_result = ModelTuneResult(inc_cfg=inc_cfg, 190 | cfgs = evaluated_cfgs, 191 | costs = evaluated_costs, 192 | inc_costs = inc_costs, 193 | inc_cfgs = inc_cfgs) 194 | 195 | model_factory, inc_cfg = self._get_model_cfg(incumbent) 196 | final_model = model_factory(inc_cfg, self.evaluator.trajs) 197 | 198 | return final_model, tune_result 199 | -------------------------------------------------------------------------------- /autompc/trajectory.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | import numpy as np 4 | from collections import namedtuple 5 | 6 | def zeros(system, size): 7 | """ 8 | Create an all zeros trajectory. 9 | 10 | Parameters 11 | ---------- 12 | system : System 13 | System for trajectory 14 | 15 | size : int 16 | Size of trajectory 17 | """ 18 | obs = np.zeros((size, system.obs_dim)) 19 | ctrls = np.zeros((size, system.ctrl_dim)) 20 | return Trajectory(system, size, obs, ctrls) 21 | 22 | def empty(system, size): 23 | """ 24 | Create a trajectory with uninitialized states 25 | and controls. If not initialized, states/controls 26 | will be non-deterministic. 27 | 28 | Parameters 29 | ---------- 30 | system : System 31 | System for trajectory 32 | 33 | size : int 34 | Size of trajectory 35 | """ 36 | obs = np.empty((size, system.obs_dim)) 37 | ctrls = np.empty((size, system.ctrl_dim)) 38 | return Trajectory(system, size, obs, ctrls) 39 | 40 | def extend(traj, obs, ctrls): 41 | """ 42 | Create a new trajectory which extends an existing trajectory 43 | by one or more timestep. 44 | 45 | Parameters 46 | ---------- 47 | traj : Trajectory 48 | Trajectory to extend 49 | 50 | obs : numpy array of shape (N, system.obs_dim) 51 | New observations 52 | 53 | ctrls : numpy array of shape (N, system.ctrl_dim) 54 | New controls 55 | """ 56 | newobs = np.concatenate([traj.obs, obs]) 57 | newctrls = np.concatenate([traj.ctrls, ctrls]) 58 | newtraj = Trajectory(traj.system, newobs.shape[0], 59 | newobs, newctrls) 60 | return newtraj 61 | 62 | TimeStep = namedtuple("TimeStep", "obs ctrl") 63 | """ 64 | TimeStep represents a particular time step of a trajectory 65 | and is returned by indexing traj[i]. 66 | 67 | .. py:attribute:: obs 68 | Observation. Numpy array of size system.obs_dim 69 | 70 | .. py:attribute:: ctrl 71 | Control. Numpy array of size system.ctrl_dim 72 | """ 73 | 74 | class Trajectory: 75 | """ 76 | The Trajectory object represents a discrete-time state and control 77 | trajectory. 78 | """ 79 | def __init__(self, system, size, obs, ctrls): 80 | """ 81 | Parameters 82 | ---------- 83 | system : System 84 | The corresponding robot system 85 | 86 | size : int 87 | Number of time steps in the trajectrory 88 | 89 | obs : numpy array of shape (size, system.obs_dim) 90 | Observations at all timesteps 91 | 92 | ctrls : numpy array of shape (size, system.ctrl_dim) 93 | Controls at all timesteps. 94 | """ 95 | self._system = system 96 | self._size = size 97 | 98 | # Check inputs 99 | if obs.shape != (size, system.obs_dim): 100 | raise ValueError("obs is wrong shape") 101 | if ctrls.shape != (size, system.ctrl_dim): 102 | raise ValueError("ctrls is wrong shape") 103 | 104 | self._obs = obs 105 | self._ctrls = ctrls 106 | 107 | def __eq__(self, other): 108 | return (self._system == other.system 109 | and self._size == other._size 110 | and np.array_equal(self._obs, other._obs) 111 | and np.array_equal(self._ctrls, other._ctrls)) 112 | 113 | def __getitem__(self, idx): 114 | if isinstance(idx, tuple): 115 | if (not isinstance(idx[0], slice) and (idx[0] < -self.size 116 | or idx[0] >= self.size)): 117 | raise IndexError("Time index out of range.") 118 | if idx[1] in self._system.observations: 119 | obs_idx = self._system.observations.index(idx[1]) 120 | return self._obs[idx[0], obs_idx] 121 | elif idx[1] in self._system.controls: 122 | ctrl_idx = self._system.controls.index(idx[1]) 123 | return self._ctrls[idx[0], ctrl_idx] 124 | else: 125 | raise IndexError("Unknown label") 126 | elif isinstance(idx, slice): 127 | #if idx.start < -self.size or idx.stop >= self.size: 128 | # raise IndexError("Time index out of range.") 129 | obs = self._obs[idx, :] 130 | ctrls = self._ctrls[idx, :] 131 | return Trajectory(self._system, obs.shape[0], obs, ctrls) 132 | else: 133 | if idx < -self.size or idx >= self.size: 134 | raise IndexError("Time index out of range.") 135 | return TimeStep(self._obs[idx,:], self._ctrls[idx,:]) 136 | 137 | def __setitem__(self, idx, val): 138 | if isinstance(idx, tuple): 139 | if isinstance(idx[0], int): 140 | if idx[0] < -self.size or idx[0] >= self.size: 141 | raise IndexError("Time index out of range.") 142 | if idx[1] in self._system.observations: 143 | obs_idx = self._system.observations.index(idx[1]) 144 | self._obs[idx[0], obs_idx] = val 145 | elif idx[1] in self._system.controls: 146 | ctrl_idx = self._system.controls.index(idx[1]) 147 | self._ctrls[idx[0], ctrl_idx] = val 148 | else: 149 | raise IndexError("Unknown label") 150 | elif isinstance(idx, int): 151 | raise IndexError("Cannot assign to time steps.") 152 | else: 153 | raise IndexError("Unknown index type") 154 | 155 | def __len__(self): 156 | return self._size 157 | 158 | def __str__(self): 159 | return "Trajectory, length={}, system={}".format(self._size,self._system) 160 | 161 | @property 162 | def system(self): 163 | """ 164 | Get trajectory System object. 165 | """ 166 | return self._system 167 | 168 | @property 169 | def size(self): 170 | """ 171 | Number of time steps in trajectory 172 | """ 173 | return self._size 174 | 175 | @property 176 | def obs(self): 177 | """ 178 | Get trajectory observations as a numpy array of 179 | shape (size, self.system.obs_dim) 180 | """ 181 | return self._obs 182 | 183 | @obs.setter 184 | def obs(self, obs): 185 | if obs.shape != (self._size, self._system.obs_dim): 186 | raise ValueError("obs is wrong shape") 187 | self._obs = obs[:] 188 | 189 | @property 190 | def ctrls(self): 191 | """ 192 | Get trajectory controls as a numpy array of 193 | shape (size, self.system.ctrl_dim) 194 | """ 195 | return self._ctrls 196 | 197 | @ctrls.setter 198 | def ctrls(self, ctrls): 199 | if ctrls.shape != (self._size, self._system.ctrl_dim): 200 | raise ValueError("ctrls is wrong shape") 201 | self._ctrls = ctrls[:] 202 | -------------------------------------------------------------------------------- /tests/test_pipeline.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-01-24 2 | 3 | # Standard library includes 4 | import unittest 5 | from pdb import set_trace 6 | 7 | # Internal library includes 8 | import autompc as ampc 9 | from autompc.sysid import SINDyFactory, SINDy 10 | from autompc.costs import QuadCostFactory, QuadCost, GaussRegFactory, SumCost 11 | from autompc.tasks import Task 12 | from autompc.control import IterativeLQRFactory, IterativeLQR 13 | from autompc.pipeline import Pipeline 14 | 15 | # External library includes 16 | import numpy as np 17 | import ConfigSpace as CS 18 | 19 | def doubleint_dynamics(y, u): 20 | """ 21 | Parameters 22 | ---------- 23 | y : states 24 | u : control 25 | 26 | Returns 27 | ------- 28 | A list describing the dynamics of the cart cart pole 29 | """ 30 | x, dx = y 31 | return np.array([dx, u]) 32 | 33 | def dt_doubleint_dynamics(y,u,dt): 34 | y += dt * doubleint_dynamics(y,u[0]) 35 | return y 36 | 37 | def uniform_random_generate(system, task, dynamics, rng, init_min, init_max, 38 | traj_len, n_trajs): 39 | trajs = [] 40 | for _ in range(n_trajs): 41 | state0 = [rng.uniform(minval, maxval, 1)[0] for minval, maxval 42 | in zip(init_min, init_max)] 43 | y = state0[:] 44 | traj = ampc.zeros(system, traj_len) 45 | traj.obs[:] = y 46 | umin, umax = task.get_ctrl_bounds().T 47 | for i in range(traj_len): 48 | traj[i].obs[:] = y 49 | u = rng.uniform(umin, umax, 1) 50 | y = dynamics(y, u) 51 | traj[i].ctrl[:] = u 52 | trajs.append(traj) 53 | return trajs 54 | 55 | class PipelineTest(unittest.TestCase): 56 | def setUp(self): 57 | simple_sys = ampc.System(["x", "y"], ["u"]) 58 | simple_sys.dt = 0.05 59 | self.system = simple_sys 60 | self.model_factory = SINDyFactory(self.system) 61 | self.cost_factory = QuadCostFactory(self.system) 62 | self.controller_factory = IterativeLQRFactory(self.system) 63 | 64 | # Initialize task 65 | Q = np.eye(2) 66 | R = np.eye(1) 67 | F = np.eye(2) 68 | cost = QuadCost(self.system, Q, R, F, goal=[-1,0]) 69 | self.task = Task(self.system) 70 | self.task.set_cost(cost) 71 | self.task.set_ctrl_bound("u", -20.0, 20.0) 72 | 73 | rng = np.random.default_rng(42) 74 | dynamics = lambda x, u: dt_doubleint_dynamics(x, u, dt=0.05) 75 | self.trajs = uniform_random_generate(self.system, self.task, 76 | dynamics, rng, init_min=-np.ones(2), 77 | init_max=np.ones(2), traj_len=100, n_trajs=100) 78 | 79 | 80 | def test_full_config_space(self): 81 | pipeline = Pipeline(self.system, self.model_factory, 82 | self.cost_factory, self.controller_factory) 83 | pipeline_cs = pipeline.get_configuration_space() 84 | model_cs = self.model_factory.get_configuration_space() 85 | cost_cs = self.cost_factory.get_configuration_space() 86 | controller_cs = self.controller_factory.get_configuration_space() 87 | 88 | pipeline_hns = pipeline_cs.get_hyperparameter_names() 89 | combined_hns = (["_model:" + hn for hn in model_cs.get_hyperparameter_names()] 90 | + ["_cost:" + hn for hn in cost_cs.get_hyperparameter_names()] 91 | + ["_ctrlr:" + hn for hn in controller_cs. 92 | get_hyperparameter_names()]) 93 | 94 | self.assertEqual(set(pipeline_hns), set(combined_hns)) 95 | 96 | def test_config_space_fixed_model(self): 97 | model_cs = self.model_factory.get_configuration_space() 98 | model_cfg = model_cs.get_default_configuration() 99 | model = self.model_factory(model_cfg, self.trajs) 100 | self.assertTrue(isinstance(model, SINDy)) 101 | 102 | pipeline = Pipeline(self.system, model, 103 | self.cost_factory, self.controller_factory) 104 | pipeline_cs = pipeline.get_configuration_space() 105 | model_cs = self.model_factory.get_configuration_space() 106 | cost_cs = self.cost_factory.get_configuration_space() 107 | controller_cs = self.controller_factory.get_configuration_space() 108 | 109 | pipeline_hns = pipeline_cs.get_hyperparameter_names() 110 | combined_hns = (["_cost:" + hn for hn in cost_cs.get_hyperparameter_names()] 111 | + ["_ctrlr:" + hn for hn in controller_cs. 112 | get_hyperparameter_names()]) 113 | 114 | self.assertEqual(set(pipeline_hns), set(combined_hns)) 115 | 116 | def test_config_space_fixed_cost(self): 117 | cost = self.task.get_cost() 118 | 119 | pipeline = Pipeline(self.system, self.model_factory, 120 | cost, self.controller_factory) 121 | pipeline_cs = pipeline.get_configuration_space() 122 | model_cs = self.model_factory.get_configuration_space() 123 | cost_cs = self.cost_factory.get_configuration_space() 124 | controller_cs = self.controller_factory.get_configuration_space() 125 | 126 | pipeline_hns = pipeline_cs.get_hyperparameter_names() 127 | combined_hns = (["_model:" + hn for hn in model_cs.get_hyperparameter_names()] 128 | + ["_ctrlr:" + hn for hn in controller_cs. 129 | get_hyperparameter_names()]) 130 | 131 | self.assertEqual(set(pipeline_hns), set(combined_hns)) 132 | 133 | def test_pipeline_call(self): 134 | pipeline = Pipeline(self.system, self.model_factory, 135 | self.cost_factory, self.controller_factory) 136 | pipeline_cs = pipeline.get_configuration_space() 137 | pipeline_cs.seed(100) 138 | pipeline_cfg = pipeline_cs.sample_configuration() 139 | controller, task, model = pipeline(pipeline_cfg, self.task, self.trajs) 140 | 141 | self.assertIsInstance(controller, IterativeLQR) 142 | self.assertIsInstance(task, Task) 143 | self.assertIsInstance(model, SINDy) 144 | 145 | self.assertEqual(controller.horizon, pipeline_cfg["_ctrlr:horizon"]) 146 | 147 | cost = task.get_cost() 148 | Q, R, F = cost._Q, cost._R, cost._F 149 | def str_to_bool(str): 150 | return str == "true" 151 | self.assertTrue(np.array_equal(Q, np.diag([pipeline_cfg["_cost:x_Q"], pipeline_cfg["_cost:y_Q"]]))) 152 | self.assertTrue(np.array_equal(F, np.diag([pipeline_cfg["_cost:x_F"], pipeline_cfg["_cost:y_F"]]))) 153 | self.assertTrue(np.array_equal(R, np.diag([pipeline_cfg["_cost:u_R"]]))) 154 | self.assertEqual(model.method, pipeline_cfg["_model:method"]) 155 | self.assertEqual(model.poly_basis, str_to_bool(pipeline_cfg["_model:poly_basis"])) 156 | self.assertEqual(model.poly_cross_terms, str_to_bool(pipeline_cfg["_model:poly_cross_terms"])) 157 | self.assertEqual(model.poly_degree, pipeline_cfg["_model:poly_degree"]) 158 | self.assertEqual(model.threshold, pipeline_cfg["_model:threshold"]) 159 | self.assertEqual(model.time_mode, pipeline_cfg["_model:time_mode"]) 160 | self.assertEqual(model.trig_basis, str_to_bool(pipeline_cfg["_model:trig_basis"])) 161 | self.assertEqual(model.trig_freq, pipeline_cfg["_model:trig_freq"]) -------------------------------------------------------------------------------- /autompc/pipeline.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu), 2021-01-25 2 | 3 | # Standard library includes 4 | import copy 5 | from pdb import set_trace 6 | 7 | # Internal library includes 8 | from .utils.cs_utils import * 9 | from .sysid.model import ModelFactory, Model 10 | from .control.controller import Controller, ControllerFactory 11 | from .costs.cost import Cost 12 | from .costs.cost_factory import CostFactory 13 | 14 | # External library includes 15 | import numpy as np 16 | import ConfigSpace as CS 17 | import ConfigSpace.hyperparameters as CSH 18 | import ConfigSpace.conditions as CSC 19 | 20 | class Pipeline: 21 | """ 22 | The Pipeline class represents a collection of MPC components, including 23 | the model, controller, and cost function. A Pipeline can provide 24 | the joint configuration space over its constituent components, and 25 | can instantiate an MPC given a configuration. 26 | """ 27 | def __init__(self, system, *components): 28 | """ 29 | Parameters 30 | ---------- 31 | system : System 32 | Corresponding robot system 33 | 34 | components : List of models, controllers, costs and corresponding factories. 35 | The set of components which make up the pipeline: the model, the controller, 36 | and the cost. For each of these components, you can either pass the factory 37 | or the instantiated version. For example, you must pass either a Controller 38 | or a ControllerFactory, but not both. If the factory is passed, than its 39 | hyperparameters will become part of the joint configuration space. If the 40 | instantiated version is passed, the component will be treated as fixed in 41 | the pipeline. 42 | """ 43 | self.system = system 44 | self.model = None 45 | self.model_factory = None 46 | self.controller = None 47 | self.controller_factory = None 48 | self.cost = None 49 | self.cost_factory = None 50 | 51 | for component in components: 52 | if isinstance(component, Model): 53 | if self.model or self.model_factory: 54 | raise ValueError("Pipeline cannot contain multiple models or " 55 | + "model factories.") 56 | self.model = component 57 | if isinstance(component, ModelFactory): 58 | if self.model or self.model_factory: 59 | raise ValueError("Pipeline cannot contain multiple models or " 60 | + "model factories.") 61 | self.model_factory = component 62 | if isinstance(component, Controller): 63 | if self.controller or self.controller_factory: 64 | raise ValueError("Pipeline cannot contain multple controllers or " 65 | + "controller factories.") 66 | self.controller = component 67 | if isinstance(component, ControllerFactory): 68 | if self.controller or self.controller_factory: 69 | raise ValueError("Pipeline cannot contain multple controllers or " 70 | + "controller factories.") 71 | self.controller_factory = component 72 | if isinstance(component, Cost): 73 | if self.cost or self.cost_factory: 74 | raise ValueError("Pipeline cannot contain multple costs or " 75 | + "cost factories.") 76 | self.cost = component 77 | if isinstance(component, CostFactory): 78 | if self.cost or self.cost_factory: 79 | raise ValueError("Pipeline cannot contain multple costs or " 80 | + "cost factories.") 81 | self.cost_factory = component 82 | 83 | if not (self.model or self.model_factory): 84 | raise ValueError("Pipeline must contain model or model factory") 85 | if not (self.controller or self.controller_factory): 86 | raise ValueError("Pipeline must contain controller or controller factory") 87 | if not (self.cost or self.cost_factory): 88 | raise ValueError("Pipeline must contain cost or cost factory") 89 | 90 | def get_configuration_space(self): 91 | """ 92 | Return the pipeline configuration space. 93 | """ 94 | cs = CS.ConfigurationSpace() 95 | if self.model_factory: 96 | model_cs = self.model_factory.get_configuration_space() 97 | add_configuration_space(cs, "_model", model_cs) 98 | if self.controller_factory: 99 | controller_cs = self.controller_factory.get_configuration_space() 100 | add_configuration_space(cs, "_ctrlr", controller_cs) 101 | if self.cost_factory: 102 | cost_factory_cs = self.cost_factory.get_configuration_space() 103 | add_configuration_space(cs, "_cost", cost_factory_cs) 104 | 105 | return cs 106 | 107 | def __call__(self, cfg, task, trajs, model=None): 108 | """ 109 | Instantiate the MPC. 110 | 111 | Parameters 112 | ---------- 113 | cfg : Configuration 114 | Configuration from the joint pipeline ConfigurationSpace 115 | 116 | task : Task 117 | Task which the MPC will solve 118 | 119 | trajs : List of Trajectory 120 | System ID training set 121 | 122 | model : Model 123 | A pre-trained model can be passed here which overrides 124 | the configuration. Default is None. 125 | 126 | Returns 127 | ------- 128 | controller : Controller 129 | The MPC controller 130 | 131 | task : Task 132 | The task with the instantiated cost 133 | 134 | model : Model 135 | The instantiated and trained model 136 | """ 137 | # First instantiate and train the model 138 | if not model: 139 | if self.model: 140 | model = self.model 141 | else: 142 | model_cs = self.model_factory.get_configuration_space() 143 | model_cfg = model_cs.get_default_configuration() 144 | set_subspace_configuration(cfg, "_model", model_cfg) 145 | model = self.model_factory(model_cfg, trajs) 146 | 147 | # Then create the objective function 148 | if self.cost: 149 | cost = self.cost 150 | else: 151 | cost_cs = self.cost_factory.get_configuration_space() 152 | cost_cfg = cost_cs.get_default_configuration() 153 | set_subspace_configuration(cfg, "_cost", cost_cfg) 154 | cost = self.cost_factory(cost_cfg, task, trajs) 155 | 156 | new_task = copy.deepcopy(task) 157 | new_task.set_cost(cost) 158 | 159 | # Then initialize the controller 160 | if self.controller: 161 | controller = self.controller 162 | else: 163 | controller_cs = self.controller_factory.get_configuration_space() 164 | controller_cfg = controller_cs.get_default_configuration() 165 | set_subspace_configuration(cfg, "_ctrlr", controller_cfg) 166 | controller = self.controller_factory(controller_cfg, new_task, model) 167 | 168 | return controller, new_task, model 169 | -------------------------------------------------------------------------------- /autompc/tasks/task.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | import numpy as np 4 | 5 | class Task: 6 | """ 7 | Defines a control task to be solved 8 | """ 9 | def __init__(self, system): 10 | """ 11 | Task constructor 12 | 13 | Parameters 14 | ---------- 15 | sytem : System 16 | Robot system for which task is defined. 17 | """ 18 | self.system = system 19 | 20 | # Initialize obs and control bounds 21 | self._obs_bounds = np.zeros((system.obs_dim, 2)) 22 | for i in range(system.obs_dim): 23 | self._obs_bounds[i, 0] = -np.inf 24 | self._obs_bounds[i, 1] = np.inf 25 | 26 | self._ctrl_bounds = np.zeros((system.ctrl_dim, 2)) 27 | for i in range(system.ctrl_dim): 28 | self._ctrl_bounds[i, 0] = -np.inf 29 | self._ctrl_bounds[i, 1] = np.inf 30 | 31 | self._eq_cons = [] 32 | self._ineq_cons = [] 33 | self._term_eq_cons = [] 34 | self._term_ineq_cons = [] 35 | self._init_eq_cons = [] 36 | self._init_ineq_cons = [] 37 | 38 | self._init_obs = None 39 | self._term_cond = None 40 | self._num_steps = None 41 | 42 | def set_num_steps(self, num_steps): 43 | """ 44 | Sets maximum number of steps as the task terminiation 45 | condition. 46 | 47 | Parameters 48 | ---------- 49 | num_steps : int 50 | Maximum number of steps. 51 | """ 52 | self._term_cond = lambda traj: len(traj) >= num_steps 53 | self._num_steps = num_steps 54 | 55 | def has_num_steps(self): 56 | """ 57 | Check whether task has a maximum number of steps for the 58 | task. 59 | 60 | Returns 61 | ------- 62 | : bool 63 | True if maximum number of steps is set 64 | """ 65 | return self._num_steps is not None 66 | 67 | def get_num_steps(self): 68 | """ 69 | Returns the maxium number steps if available. None otherwise. 70 | """ 71 | return self._num_steps 72 | 73 | def term_cond(self, traj): 74 | """ 75 | Checks the task termination condition. 76 | 77 | Parameters 78 | ---------- 79 | traj : Trajectory 80 | Trajectory to check termination condition. 81 | 82 | Returns 83 | ------- 84 | : bool 85 | True if termination condition met. 86 | """ 87 | if self._term_cond is not None: 88 | return self._term_cond(traj) 89 | else: 90 | return False 91 | 92 | def set_term_cond(self, term_cond): 93 | """ 94 | Set the task termination condition 95 | 96 | Parameters 97 | ---------- 98 | term_cond : Function, Trajectory -> bool 99 | Termination condition function. 100 | """ 101 | self._term_cond = term_cond 102 | 103 | def set_cost(self, cost): 104 | """ 105 | Sets the task cost 106 | 107 | Parameters 108 | ---------- 109 | cost : Cost 110 | Cost to be set 111 | """ 112 | self.cost = cost 113 | 114 | def get_cost(self): 115 | """ 116 | Get the task cost 117 | 118 | Returns 119 | ------- 120 | : Cost 121 | Task cost 122 | """ 123 | return self.cost 124 | 125 | def set_init_obs(self, init_obs): 126 | """ 127 | Sets the initial observation for the task. 128 | 129 | Parameters 130 | ---------- 131 | init_obs : numpy array of size self.system.obs_dim 132 | Initial observation 133 | """ 134 | self._init_obs = init_obs[:] 135 | 136 | def get_init_obs(self): 137 | """ 138 | Get the initial observation for the task 139 | 140 | Returns : numpy array of size self.system.obs_dim 141 | Initial observation 142 | """ 143 | if self._init_obs is not None: 144 | return self._init_obs[:] 145 | else: 146 | return None 147 | 148 | # Handle bounds 149 | def set_obs_bound(self, obs_label, lower, upper): 150 | """ 151 | Set a bound for one dimension of the observation 152 | 153 | Parameters 154 | ---------- 155 | obs_label : string 156 | Name of observation dimension to be bounded 157 | 158 | lower : float 159 | Lower bound 160 | 161 | upper : float 162 | Upper bound 163 | """ 164 | idx = self.system.observations.index(obs_label) 165 | self._obs_bounds[idx,:] = [lower, upper] 166 | 167 | def set_obs_bounds(self, lowers, uppers): 168 | """ 169 | Set bound for all observation dimensions 170 | 171 | Parameters 172 | ---------- 173 | lowers : numpy array of size self.system.obs_dim 174 | Lower bounds 175 | 176 | uppers : numpy array of size self.system.obs_dim 177 | Upper bounds 178 | """ 179 | self._obs_bounds[:,0] = lowers 180 | self._obs_bounds[:,1] = uppers 181 | 182 | def set_ctrl_bound(self, ctrl_label, lower, upper): 183 | """ 184 | Set a bound for one dimension of the control 185 | 186 | Parameters 187 | ---------- 188 | ctrl_label : string 189 | Name of control dimension to be bounded 190 | 191 | lower : float 192 | Lower bound 193 | 194 | upper : float 195 | Upper bound 196 | """ 197 | idx = self.system.controls.index(ctrl_label) 198 | self._ctrl_bounds[idx,:] = [lower, upper] 199 | 200 | def set_ctrl_bounds(self, lowers, uppers): 201 | """ 202 | Set bound for all control dimensions 203 | 204 | Parameters 205 | ---------- 206 | lowers : numpy array of size self.system.ctrl_dim 207 | Lower bounds 208 | 209 | uppers : numpy array of size self.system.ctrl_dim 210 | Upper bounds 211 | """ 212 | self._ctrl_bounds[:,0] = lowers 213 | self._ctrl_bounds[:,1] = uppers 214 | 215 | def are_obs_bounded(self): 216 | """ 217 | Check whether task has observation bounds 218 | 219 | Returns 220 | ------- 221 | : bool 222 | True if any observation dimension is bounded 223 | """ 224 | for i in range(self.system.obs_dim): 225 | if (self._obs_bounds[i, 0] != -np.inf 226 | or self._obs_bounds[i, 1] != np.inf): 227 | return True 228 | return False 229 | 230 | def are_ctrl_bounded(self): 231 | """ 232 | Check whether task has control bounds 233 | 234 | Returns 235 | ------- 236 | : bool 237 | True if any control dimension is bounded 238 | """ 239 | for i in range(self.system.ctrl_dim): 240 | if (self._ctrl_bounds[i, 0] != -np.inf 241 | or self._ctrl_bounds[i, 1] != np.inf): 242 | return True 243 | return False 244 | 245 | def get_obs_bounds(self): 246 | """ 247 | Get observation bounds. If unbounded, lower and upper bound 248 | are -np.inf and +np.inf respectively. 249 | 250 | Returns 251 | ------- 252 | : numpy array of shape (self.system.obs_dim, 2) 253 | Observation bounds 254 | """ 255 | return self._obs_bounds.copy() 256 | 257 | def get_ctrl_bounds(self): 258 | """ 259 | Get control bounds. If unbounded, lower and upper bound 260 | are -np.inf and +np.inf respectively. 261 | 262 | Returns 263 | ------- 264 | : numpy array of shape (self.system.ctrl_dim, 2) 265 | Control bounds 266 | """ 267 | return self._ctrl_bounds.copy() 268 | -------------------------------------------------------------------------------- /autompc/costs/cost.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards, (wre2@illinois.edu) 2 | 3 | import numpy as np 4 | 5 | from abc import ABC, abstractmethod 6 | 7 | class Cost(ABC): 8 | """ 9 | Base class for cost functions. 10 | """ 11 | def __init__(self, system): 12 | """ 13 | Create cost 14 | 15 | Parameters 16 | ---------- 17 | system : System 18 | Robot system for which cost will be evaluated 19 | """ 20 | self.system = system 21 | self._is_quad = False 22 | self._is_convex = False 23 | self._is_diff = False 24 | self._is_twice_diff = False 25 | self._has_goal = False 26 | 27 | def __call__(self, traj): 28 | """ 29 | Evaluate cost on whole trajectory 30 | 31 | Parameters 32 | ---------- 33 | traj : Trajectory 34 | Trajectory to evaluate 35 | """ 36 | cost = 0.0 37 | for i in range(len(traj)): 38 | cost += self.eval_obs_cost(traj[i].obs) 39 | cost += self.eval_ctrl_cost(traj[i].ctrl) 40 | cost += self.eval_term_obs_cost(traj[-1].obs) 41 | return cost 42 | 43 | def get_cost_matrices(self): 44 | """ 45 | Return quadratic Q, R, and F matrices. Raises exception 46 | for non-quadratic cost. 47 | """ 48 | if self.is_quad: 49 | return np.copy(self._Q), np.copy(self._R), np.copy(self._F) 50 | else: 51 | raise ValueError("Cost is not quadratic.") 52 | 53 | def get_goal(self): 54 | """ 55 | Returns the cost goal state if available. Raises exception 56 | if cost does not have goal. 57 | 58 | Returns : numpy array of size self.system.obs_dim 59 | Goal state 60 | """ 61 | if self.has_goal: 62 | return np.copy(self._goal) 63 | else: 64 | raise ValueError("Cost does not have goal") 65 | 66 | def eval_obs_cost(self, obs): 67 | """ 68 | Evaluates observation cost at a particular time step. 69 | Raises exception if not implemented. 70 | 71 | Parameters 72 | ---------- 73 | obs : self.system.obs_dim 74 | Observation 75 | 76 | Returns : float 77 | Cost 78 | """ 79 | if self.is_quad: 80 | obst = obs - self._goal 81 | return obst.T @ self._Q @ obst 82 | else: 83 | raise NotImplementedError 84 | 85 | def eval_obs_cost_diff(self, obs): 86 | """ 87 | Evaluates the observation cost at a particular time 88 | steps and computes Jacobian. Raises exception if not 89 | implemented. 90 | 91 | Returns : (float, numpy array of size self.system.obs_dim) 92 | Cost, Jacobian 93 | """ 94 | if self.is_quad: 95 | obst = obs - self._goal 96 | return obst.T @ self._Q @ obst, (self._Q + self._Q.T) @ obst 97 | else: 98 | raise NotImplementedError 99 | 100 | def eval_obs_cost_hess(self, obs): 101 | """ 102 | Evaluates the observation cost at a particular time 103 | steps and computes Jacobian and Hessian. Raises exception if not 104 | implemented. 105 | 106 | Returns : (float, numpy array of size self.system.obs_dim, 107 | numpy array of shape (self.system.obs_dim, self.system.obsd_im)) 108 | Cost, Jacobian, Hessian 109 | """ 110 | if self.is_quad: 111 | obst = obs - self._goal 112 | return (obst.T @ self._Q @ obst, 113 | (self._Q + self._Q.T) @ obst, 114 | self._Q + self._Q.T) 115 | else: 116 | raise NotImplementedError 117 | 118 | def eval_ctrl_cost(self, ctrl): 119 | """ 120 | Evaluates control cost at a particular time step. 121 | Raises exception if not implemented. 122 | 123 | Parameters 124 | ---------- 125 | obs : self.system.ctrl_dim 126 | Control 127 | 128 | Returns : float 129 | Cost 130 | """ 131 | if self.is_quad: 132 | return ctrl.T @ self._R @ ctrl 133 | else: 134 | raise NotImplementedError 135 | 136 | def eval_ctrl_cost_diff(self, ctrl): 137 | """ 138 | Evaluates the control cost at a particular time 139 | step and computes Jacobian. Raises exception if not 140 | implemented. 141 | 142 | Returns : (float, numpy array of size self.system.ctrl_dim) 143 | Cost, Jacobian 144 | """ 145 | if self.is_quad: 146 | return ctrl.T @ self._R @ ctrl, (self._R + self._R.T) @ ctrl 147 | else: 148 | raise NotImplementedError 149 | 150 | def eval_ctrl_cost_hess(self, ctrl): 151 | """ 152 | Evaluates the control cost at a particular time 153 | steps and computes Jacobian and Hessian. Raises exception if not 154 | implemented. 155 | 156 | Returns : (float, numpy array of size self.system.ctrl_dim, numpy array of shape (self.system.ctrl_dim, self.system.ctrl_dim)) 157 | Cost, Jacobian, Hessian 158 | """ 159 | if self.is_quad: 160 | return (ctrl.T @ self._R @ ctrl, 161 | (self._R + self._R.T) @ ctrl, 162 | self._R + self._R.T) 163 | else: 164 | raise NotImplementedError 165 | 166 | def eval_term_obs_cost(self, obs): 167 | """ 168 | Evaluates terminal observation cost. 169 | Raises exception if not implemented. 170 | 171 | Parameters 172 | ---------- 173 | obs : self.system.obs_dim 174 | Observation 175 | 176 | Returns : float 177 | Cost 178 | """ 179 | if self.is_quad: 180 | obst = obs - self._goal 181 | return obst.T @ self._F @ obst 182 | else: 183 | raise NotImplementedError 184 | 185 | def eval_term_obs_cost_diff(self, obs): 186 | """ 187 | Evaluates the terminal observation cost 188 | and computes Jacobian. Raises exception if not 189 | implemented. 190 | 191 | Returns : (float, numpy array of size self.system.obs_dim) 192 | Cost, Jacobian 193 | """ 194 | if self.is_quad: 195 | return obs.T @ self._F @ obs, (self._F + self._F.T) @ obs 196 | else: 197 | raise NotImplementedError 198 | 199 | def eval_term_obs_cost_hess(self, obs): 200 | """ 201 | Evaluates the terminal observation cost 202 | and computes Jacobian and Hessian. Raises exception if not 203 | implemented. 204 | 205 | Returns : (float, numpy array of size self.system.obs_dim, numpy array of shape (self.system.obs_dim, self.system.obsd_im)) 206 | Cost, Jacobian, Hessian 207 | """ 208 | if self.is_quad: 209 | return (obs.T @ self._F @ obs, 210 | (self._F + self._F.T) @ obs, 211 | self._F + self._F.T) 212 | else: 213 | raise NotImplementedError 214 | 215 | @property 216 | def is_quad(self): 217 | """ 218 | True if cost is quadratic. 219 | """ 220 | return self._is_quad 221 | 222 | @property 223 | def is_convex(self): 224 | """ 225 | True if cost is convex. 226 | """ 227 | return self._is_convex 228 | 229 | @property 230 | def is_diff(self): 231 | """ 232 | True if cost is differentiable. 233 | """ 234 | return self._is_diff 235 | 236 | @property 237 | def is_twice_diff(self): 238 | """ 239 | True if cost is twice differentiable 240 | """ 241 | return self._is_twice_diff 242 | 243 | @property 244 | def has_goal(self): 245 | """ 246 | True if cost has goal 247 | """ 248 | return self._has_goal 249 | 250 | def __add__(self, other): 251 | from .sum_cost import SumCost 252 | if isinstance(other, SumCost): 253 | return other.__radd__(self) 254 | else: 255 | return SumCost(self.system, [self, other]) 256 | -------------------------------------------------------------------------------- /autompc/sysid/koopman.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import numpy.linalg as la 3 | import scipy.linalg as sla 4 | from pdb import set_trace 5 | from sklearn.linear_model import Lasso 6 | 7 | from .model import Model, ModelFactory 8 | from .stable_koopman import stabilize_discrete 9 | 10 | import ConfigSpace as CS 11 | import ConfigSpace.hyperparameters as CSH 12 | import ConfigSpace.conditions as CSC 13 | 14 | class KoopmanFactory(ModelFactory): 15 | """ 16 | This class identifies Koopman models of the form :math:`\dot{\Psi}(x) = A\Psi(x) + Bu`. 17 | Given states :math:`x \in \mathbb{R}^n`, :math:`\Psi(x) \in \mathbb{R}^N` ---termed Koopman 18 | basis functions--- are used to lift the dynamics in a higher-dimensional space 19 | where nonlinear functions of the system states evolve linearly. The identification 20 | of the Koopman model, specified by the A and B matrices, can be done in various ways, 21 | such as solving a least squares solution :math:`\|\dot{\Psi}(x) - [A, B][\Psi(x),u]^T\|`. 22 | 23 | The choice of the basis functions is an open research question. In this implementation, 24 | we choose basis functions that depend only on the system states and not the control, 25 | in order to derive a representation that is amenable for LQR control. 26 | 27 | 28 | Hyperparameters: 29 | 30 | - *method* (Type: str, Choices: ["lstsq", "lasso", "stable"]): Method for training Koopman 31 | operator. 32 | - *lasso_alpha* (Type: float, Low: 10^-10, High: 10^2, Defalt: 1.0): α parameter for Lasso 33 | regression. (Conditioned on method="lasso"). 34 | - *poly_basis* (Type: bool): Whether to use polynomial basis functions. 35 | - *poly_degree* (Type: int, Low: 2, High: 8, Default: 3): Maximum degree of polynomial basis 36 | functions. (Conditioned on poly_basis="true"). 37 | - *trig_basis* (Type: bool): Whether to use trig basis functions. 38 | - *trig_freq* (Type: int, Low: 1, High: 8, Default: 1): Maximum frequency of trig functions. 39 | - *product_terms* (Type: bool): Whether to include cross-product terms. 40 | """ 41 | def __init__(self, *args, **kwargs): 42 | super().__init__(*args, **kwargs) 43 | self.Model = Koopman 44 | self.name = "Koopman" 45 | 46 | def get_configuration_space(self): 47 | cs = CS.ConfigurationSpace() 48 | method = CSH.CategoricalHyperparameter("method", choices=["lstsq", "lasso", 49 | "stable"]) 50 | lasso_alpha = CSH.UniformFloatHyperparameter("lasso_alpha", 51 | lower=1e-10, upper=1e2, default_value=1.0, log=True) 52 | use_lasso_alpha = CSC.InCondition(child=lasso_alpha, parent=method, 53 | values=["lasso"]) 54 | 55 | poly_basis = CSH.CategoricalHyperparameter("poly_basis", 56 | choices=["true", "false"], default_value="false") 57 | poly_degree = CSH.UniformIntegerHyperparameter("poly_degree", lower=2, upper=8, 58 | default_value=3) 59 | use_poly_degree = CSC.InCondition(child=poly_degree, parent=poly_basis, 60 | values=["true"]) 61 | 62 | trig_basis = CSH.CategoricalHyperparameter("trig_basis", 63 | choices=["true", "false"], default_value="false") 64 | trig_freq = CSH.UniformIntegerHyperparameter("trig_freq", lower=1, upper=8, 65 | default_value=1) 66 | use_trig_freq = CSC.InCondition(child=trig_freq, parent=trig_basis, 67 | values=["true"]) 68 | 69 | product_terms = CSH.CategoricalHyperparameter("product_terms", 70 | choices=["false"], default_value="false") 71 | 72 | 73 | cs.add_hyperparameters([method, poly_basis, poly_degree, 74 | trig_basis, trig_freq, product_terms, lasso_alpha]) 75 | cs.add_conditions([use_poly_degree, use_trig_freq, use_lasso_alpha]) 76 | 77 | return cs 78 | 79 | class Koopman(Model): 80 | def __init__(self, system, method, lasso_alpha=None, poly_basis=False, 81 | poly_degree=1, trig_basis=False, trig_freq=1, product_terms=False, 82 | use_cuda=None): 83 | super().__init__(system) 84 | 85 | self.method = method 86 | if not lasso_alpha is None: 87 | self.lasso_alpha = lasso_alpha 88 | else: 89 | self.lasso_alpha = None 90 | if type(poly_basis) == str: 91 | poly_basis = True if poly_basis == "true" else False 92 | self.poly_basis = poly_basis 93 | self.poly_degree = poly_degree 94 | if type(trig_basis) == str: 95 | trig_basis = True if trig_basis == "true" else False 96 | self.trig_basis = trig_basis 97 | self.trig_freq = trig_freq 98 | if type(product_terms) == str: 99 | self.product_terms = True if product_terms == "true" else False 100 | 101 | self.basis_funcs = [lambda x: x] 102 | if self.poly_basis: 103 | self.basis_funcs += [lambda x: x**i for i in range(2, 1+self.poly_degree)] 104 | if self.trig_basis: 105 | for i in range(1, 1+self.poly_degree): 106 | self.basis_funcs += [lambda x: np.sin(i*x), lambda x : np.cos(i*x)] 107 | 108 | def _apply_basis(self, state): 109 | tr_state = [b(x) for b in self.basis_funcs for x in state] 110 | if self.product_terms: 111 | pr_terms = [] 112 | for i, x in enumerate(tr_state): 113 | for j, y in enumerate(tr_state): 114 | if i < j: 115 | pr_terms.append(x*y) 116 | tr_state += pr_terms 117 | 118 | return np.array(tr_state) 119 | 120 | def _transform_observations(self, observations): 121 | return np.apply_along_axis(self._apply_basis, 1, observations) 122 | 123 | def traj_to_state(self, traj): 124 | return self._transform_observations(traj.obs[:])[-1,:] 125 | 126 | def traj_to_states(self, traj): 127 | return self._transform_observations(traj.obs[:]) 128 | 129 | def update_state(self, state, new_ctrl, new_obs): 130 | return self._apply_basis(new_obs) 131 | 132 | @property 133 | def state_dim(self): 134 | return len(self.basis_funcs) * self.system.obs_dim 135 | 136 | def train(self, trajs, silent=False): 137 | trans_obs = [self._transform_observations(traj.obs[:]) for traj in trajs] 138 | X = np.concatenate([obs[:-1,:] for obs in trans_obs]).T 139 | Y = np.concatenate([obs[1:,:] for obs in trans_obs]).T 140 | U = np.concatenate([traj.ctrls[:-1,:] for traj in trajs]).T 141 | 142 | n = X.shape[0] # state dimension 143 | m = U.shape[0] # control dimension 144 | 145 | XU = np.concatenate((X, U), axis = 0) # stack X and U together 146 | if self.method == "lstsq": # Least Squares Solution 147 | AB = np.dot(Y, sla.pinv2(XU)) 148 | A = AB[:n, :n] 149 | B = AB[:n, n:] 150 | elif self.method == "lasso": # Call lasso regression on coefficients 151 | print("Call Lasso") 152 | clf = Lasso(alpha=self.lasso_alpha) 153 | clf.fit(XU.T, Y.T) 154 | AB = clf.coef_ 155 | A = AB[:n, :n] 156 | B = AB[:n, n:] 157 | elif self.method == "stable": # Compute stable A, and B 158 | print("Compute Stable Koopman") 159 | # call function 160 | A, _, _, _, B, _ = stabilize_discrete(X, U, Y) 161 | A = np.real(A) 162 | B = np.real(B) 163 | 164 | self.A, self.B = A, B 165 | 166 | def pred(self, state, ctrl): 167 | xpred = self.A @ state + self.B @ ctrl 168 | return xpred 169 | 170 | def pred_batch(self, states, ctrls): 171 | statesnew = self.A @ states.T + self.B @ ctrls.T 172 | 173 | return statesnew.T 174 | 175 | def pred_diff(self, state, ctrl): 176 | xpred = self.A @ state + self.B @ ctrl 177 | 178 | return xpred, np.copy(self.A), np.copy(self.B) 179 | 180 | def to_linear(self): 181 | return np.copy(self.A), np.copy(self.B) 182 | 183 | def get_parameters(self): 184 | return {"A" : np.copy(self.A), 185 | "B" : np.copy(self.B)} 186 | 187 | def set_parameters(self, params): 188 | self.A = np.copy(params["A"]) 189 | self.B = np.copy(params["B"]) 190 | -------------------------------------------------------------------------------- /autompc/utils/cs_utils.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | import ConfigSpace as CS 4 | import ConfigSpace.hyperparameters as CSH 5 | import ConfigSpace.conditions as CSC 6 | import copy 7 | 8 | from ConfigSpace import ConfigurationSpace 9 | from ConfigSpace.hyperparameters import ( 10 | Hyperparameter, 11 | Constant, 12 | FloatHyperparameter, 13 | NumericalHyperparameter, 14 | UniformFloatHyperparameter, 15 | UniformIntegerHyperparameter, 16 | CategoricalHyperparameter, 17 | ) 18 | from ConfigSpace.conditions import ( 19 | ConditionComponent, 20 | AbstractCondition, 21 | AbstractConjunction, 22 | EqualsCondition, 23 | ) 24 | from ConfigSpace.forbidden import ( 25 | AbstractForbiddenComponent, 26 | AbstractForbiddenClause, 27 | AbstractForbiddenConjunction, 28 | ) 29 | 30 | def _get_subkey(key, delimiter): 31 | return delimiter.join(key.split(delimiter)[1:]) 32 | 33 | def set_subspace_configuration(cfg, prefix, sub_cfg, delimiter=":"): 34 | prefix = prefix + delimiter 35 | for key, val in cfg.get_dictionary().items(): 36 | if key[:len(prefix)] == prefix: 37 | sub_cfg[_get_subkey(key, delimiter)] = val 38 | 39 | def transfer_subspace_configuration(source_cfg, source_prefix, dest_cfg, dest_prefix, 40 | delimiter=":"): 41 | source_prefix = source_prefix + delimiter 42 | dest_prefix = dest_prefix + delimiter 43 | for source_key, source_val in source_cfg.get_dictionary().items(): 44 | if source_key[:len(source_prefix)] == source_prefix: 45 | key = get_subkey(source_key, delimiter) 46 | dest_cfg[dest_prefix + key] = source_val 47 | 48 | def set_parent_configuration(cfg, prefix, sub_cfg, delimiter=":"): 49 | prefix = prefix + delimiter 50 | for key, val in sub_cfg.get_dictionary().items(): 51 | cfg[prefix+key] = val 52 | 53 | def add_configuration_space(self, 54 | prefix: str, 55 | configuration_space: 'ConfigurationSpace', 56 | delimiter: str = ":", 57 | parent_hyperparameter: Hyperparameter = None 58 | ): 59 | """ 60 | Combine two configuration space by adding one the other configuration 61 | space. The contents of the configuration space, which should be added, 62 | are renamed to ``prefix`` + ``delimiter`` + old_name. 63 | 64 | Parameters 65 | ---------- 66 | prefix : str 67 | The prefix for the renamed hyperparameter | conditions | 68 | forbidden clauses 69 | configuration_space : :class:`~ConfigSpace.configuration_space.ConfigurationSpace` 70 | The configuration space which should be added 71 | delimiter : str, optional 72 | Defaults to ':' 73 | parent_hyperparameter : :ref:`Hyperparameters`, optional 74 | Adds for each new hyperparameter the condition, that 75 | ``parent_hyperparameter`` is active 76 | 77 | Returns 78 | ------- 79 | :class:`~ConfigSpace.configuration_space.ConfigurationSpace` 80 | The configuration space, which was added 81 | """ 82 | if not isinstance(configuration_space, ConfigurationSpace): 83 | raise TypeError("The method add_configuration_space must be " 84 | "called with an instance of " 85 | "ConfigSpace.configuration_space." 86 | "ConfigurationSpace.") 87 | 88 | new_parameters = [] 89 | for hp in configuration_space.get_hyperparameters(): 90 | new_parameter = copy.copy(hp) 91 | # Allow for an empty top-level parameter 92 | if new_parameter.name == '': 93 | new_parameter.name = prefix 94 | else: 95 | new_parameter.name = "%s%s%s" % (prefix, delimiter, 96 | new_parameter.name) 97 | new_parameters.append(new_parameter) 98 | self.add_hyperparameters(new_parameters) 99 | 100 | conditions_to_add = [] 101 | for condition in configuration_space.get_conditions(): 102 | new_condition = copy.copy(condition) 103 | dlcs = new_condition.get_descendant_literal_conditions() 104 | for dlc in dlcs: 105 | if dlc.child.name == prefix or dlc.child.name == '': 106 | dlc.child.name = prefix 107 | elif not dlc.child.name.startswith( 108 | "%s%s" % (prefix, delimiter)): 109 | dlc_child_name = "%s%s%s" % ( 110 | prefix, delimiter, dlc.child.name) 111 | dlc.child = self.get_hyperparameter(dlc_child_name) 112 | if dlc.parent.name == prefix or dlc.parent.name == '': 113 | dlc.parent.name = prefix 114 | elif not dlc.parent.name.startswith( 115 | "%s%s" % (prefix, delimiter)): 116 | dlc_parent_name = "%s%s%s" % ( 117 | prefix, delimiter, dlc.parent.name) 118 | dlc.parent = self.get_hyperparameter(dlc_parent_name) 119 | conditions_to_add.append(new_condition) 120 | self.add_conditions(conditions_to_add) 121 | 122 | forbiddens_to_add = [] 123 | for forbidden_clause in configuration_space.forbidden_clauses: 124 | # new_forbidden = copy.deepcopy(forbidden_clause) 125 | new_forbidden = forbidden_clause 126 | dlcs = new_forbidden.get_descendant_literal_clauses() 127 | for dlc in dlcs: 128 | if dlc.hyperparameter.name == prefix or \ 129 | dlc.hyperparameter.name == '': 130 | dlc.hyperparameter.name = prefix 131 | elif not dlc.hyperparameter.name.startswith( 132 | "%s%s" % (prefix, delimiter)): 133 | dlc.hyperparameter.name = "%s%s%s" % \ 134 | (prefix, delimiter, 135 | dlc.hyperparameter.name) 136 | forbiddens_to_add.append(new_forbidden) 137 | self.add_forbidden_clauses(forbiddens_to_add) 138 | 139 | conditions_to_add = [] 140 | if parent_hyperparameter is not None: 141 | for new_parameter in new_parameters: 142 | # Only add a condition if the parameter is a top-level 143 | # parameter of the new configuration space (this will be some 144 | # kind of tree structure). 145 | if self.get_parents_of(new_parameter): 146 | continue 147 | condition = EqualsCondition(new_parameter, 148 | parent_hyperparameter['parent'], 149 | parent_hyperparameter['value']) 150 | conditions_to_add.append(condition) 151 | self.add_conditions(conditions_to_add) 152 | 153 | def set_hyper_bounds(cs, hp_name, lower, upper): 154 | hp = cs.get_hyperparameter(hp_name) 155 | if not isinstance(hp, NumericalHyperparameter): 156 | raise ValueError("Can only call set_hyper_bounds for NumericalHyperparameter") 157 | name = hp.name 158 | default_value = hp.default_value 159 | if not (lower < default_value < upper): 160 | default_value = lower 161 | if isinstance(hp, UniformFloatHyperparameter): 162 | new_hp = CS.UniformFloatHyperparameter(name=name, lower=lower, 163 | upper=upper, default_value=default_value) 164 | if isinstance(hp, UniformIntegerHyperparameter): 165 | new_hp = CS.UniformIntegerHyperparameter(name=name, lower=lower, 166 | upper=upper, default_value=default_value) 167 | cs._hyperparameters[name] = new_hp 168 | 169 | def set_hyper_choices(cs, hp_name, choices): 170 | hp = cs.get_hyperparameter(hp_name) 171 | if not isinstance(hp, CategoricalHyperparameter): 172 | raise ValueError("Can only call set_hyper_choices for CategoricalHyperparameter") 173 | name = hp.name 174 | default_value = hp.default_value 175 | if not default_value in choices: 176 | default_value = choices[0] 177 | new_hp = CS.CategoricalHyperparameter(name=name, choices=choices, default_value=default_value) 178 | cs._hyperparameters[name] = new_hp 179 | 180 | def set_hyper_constant(cs, hp_name, value): 181 | hp = cs.get_hyperparameter(hp_name) 182 | name = hp.name 183 | new_hp = CS.Constant(name=name, value=value) 184 | cs._hyperparameters[name] = new_hp 185 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | REM Command file for Sphinx documentation 4 | 5 | if "%SPHINXBUILD%" == "" ( 6 | set SPHINXBUILD=sphinx-build 7 | ) 8 | set BUILDDIR=_build 9 | set ALLSPHINXOPTS=-d %BUILDDIR%/doctrees %SPHINXOPTS% . 10 | set I18NSPHINXOPTS=%SPHINXOPTS% . 11 | if NOT "%PAPER%" == "" ( 12 | set ALLSPHINXOPTS=-D latex_paper_size=%PAPER% %ALLSPHINXOPTS% 13 | set I18NSPHINXOPTS=-D latex_paper_size=%PAPER% %I18NSPHINXOPTS% 14 | ) 15 | 16 | if "%1" == "" goto help 17 | 18 | if "%1" == "help" ( 19 | :help 20 | echo.Please use `make ^` where ^ is one of 21 | echo. html to make standalone HTML files 22 | echo. dirhtml to make HTML files named index.html in directories 23 | echo. singlehtml to make a single large HTML file 24 | echo. pickle to make pickle files 25 | echo. json to make JSON files 26 | echo. htmlhelp to make HTML files and a HTML help project 27 | echo. qthelp to make HTML files and a qthelp project 28 | echo. devhelp to make HTML files and a Devhelp project 29 | echo. epub to make an epub 30 | echo. latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter 31 | echo. text to make text files 32 | echo. man to make manual pages 33 | echo. texinfo to make Texinfo files 34 | echo. gettext to make PO message catalogs 35 | echo. changes to make an overview over all changed/added/deprecated items 36 | echo. xml to make Docutils-native XML files 37 | echo. pseudoxml to make pseudoxml-XML files for display purposes 38 | echo. linkcheck to check all external links for integrity 39 | echo. doctest to run all doctests embedded in the documentation if enabled 40 | echo. coverage to run coverage check of the documentation if enabled 41 | goto end 42 | ) 43 | 44 | if "%1" == "clean" ( 45 | for /d %%i in (%BUILDDIR%\*) do rmdir /q /s %%i 46 | del /q /s %BUILDDIR%\* 47 | goto end 48 | ) 49 | 50 | 51 | REM Check if sphinx-build is available and fallback to Python version if any 52 | %SPHINXBUILD% 1>NUL 2>NUL 53 | if errorlevel 9009 goto sphinx_python 54 | goto sphinx_ok 55 | 56 | :sphinx_python 57 | 58 | set SPHINXBUILD=python -m sphinx.__init__ 59 | %SPHINXBUILD% 2> nul 60 | if errorlevel 9009 ( 61 | echo. 62 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 63 | echo.installed, then set the SPHINXBUILD environment variable to point 64 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 65 | echo.may add the Sphinx directory to PATH. 66 | echo. 67 | echo.If you don't have Sphinx installed, grab it from 68 | echo.http://sphinx-doc.org/ 69 | exit /b 1 70 | ) 71 | 72 | :sphinx_ok 73 | 74 | 75 | if "%1" == "html" ( 76 | %SPHINXBUILD% -b html %ALLSPHINXOPTS% %BUILDDIR%/html 77 | if errorlevel 1 exit /b 1 78 | echo. 79 | echo.Build finished. The HTML pages are in %BUILDDIR%/html. 80 | goto end 81 | ) 82 | 83 | if "%1" == "dirhtml" ( 84 | %SPHINXBUILD% -b dirhtml %ALLSPHINXOPTS% %BUILDDIR%/dirhtml 85 | if errorlevel 1 exit /b 1 86 | echo. 87 | echo.Build finished. The HTML pages are in %BUILDDIR%/dirhtml. 88 | goto end 89 | ) 90 | 91 | if "%1" == "singlehtml" ( 92 | %SPHINXBUILD% -b singlehtml %ALLSPHINXOPTS% %BUILDDIR%/singlehtml 93 | if errorlevel 1 exit /b 1 94 | echo. 95 | echo.Build finished. The HTML pages are in %BUILDDIR%/singlehtml. 96 | goto end 97 | ) 98 | 99 | if "%1" == "pickle" ( 100 | %SPHINXBUILD% -b pickle %ALLSPHINXOPTS% %BUILDDIR%/pickle 101 | if errorlevel 1 exit /b 1 102 | echo. 103 | echo.Build finished; now you can process the pickle files. 104 | goto end 105 | ) 106 | 107 | if "%1" == "json" ( 108 | %SPHINXBUILD% -b json %ALLSPHINXOPTS% %BUILDDIR%/json 109 | if errorlevel 1 exit /b 1 110 | echo. 111 | echo.Build finished; now you can process the JSON files. 112 | goto end 113 | ) 114 | 115 | if "%1" == "htmlhelp" ( 116 | %SPHINXBUILD% -b htmlhelp %ALLSPHINXOPTS% %BUILDDIR%/htmlhelp 117 | if errorlevel 1 exit /b 1 118 | echo. 119 | echo.Build finished; now you can run HTML Help Workshop with the ^ 120 | .hhp project file in %BUILDDIR%/htmlhelp. 121 | goto end 122 | ) 123 | 124 | if "%1" == "qthelp" ( 125 | %SPHINXBUILD% -b qthelp %ALLSPHINXOPTS% %BUILDDIR%/qthelp 126 | if errorlevel 1 exit /b 1 127 | echo. 128 | echo.Build finished; now you can run "qcollectiongenerator" with the ^ 129 | .qhcp project file in %BUILDDIR%/qthelp, like this: 130 | echo.^> qcollectiongenerator %BUILDDIR%\qthelp\AutoMPC.qhcp 131 | echo.To view the help file: 132 | echo.^> assistant -collectionFile %BUILDDIR%\qthelp\AutoMPC.ghc 133 | goto end 134 | ) 135 | 136 | if "%1" == "devhelp" ( 137 | %SPHINXBUILD% -b devhelp %ALLSPHINXOPTS% %BUILDDIR%/devhelp 138 | if errorlevel 1 exit /b 1 139 | echo. 140 | echo.Build finished. 141 | goto end 142 | ) 143 | 144 | if "%1" == "epub" ( 145 | %SPHINXBUILD% -b epub %ALLSPHINXOPTS% %BUILDDIR%/epub 146 | if errorlevel 1 exit /b 1 147 | echo. 148 | echo.Build finished. The epub file is in %BUILDDIR%/epub. 149 | goto end 150 | ) 151 | 152 | if "%1" == "latex" ( 153 | %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex 154 | if errorlevel 1 exit /b 1 155 | echo. 156 | echo.Build finished; the LaTeX files are in %BUILDDIR%/latex. 157 | goto end 158 | ) 159 | 160 | if "%1" == "latexpdf" ( 161 | %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex 162 | cd %BUILDDIR%/latex 163 | make all-pdf 164 | cd %~dp0 165 | echo. 166 | echo.Build finished; the PDF files are in %BUILDDIR%/latex. 167 | goto end 168 | ) 169 | 170 | if "%1" == "latexpdfja" ( 171 | %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex 172 | cd %BUILDDIR%/latex 173 | make all-pdf-ja 174 | cd %~dp0 175 | echo. 176 | echo.Build finished; the PDF files are in %BUILDDIR%/latex. 177 | goto end 178 | ) 179 | 180 | if "%1" == "text" ( 181 | %SPHINXBUILD% -b text %ALLSPHINXOPTS% %BUILDDIR%/text 182 | if errorlevel 1 exit /b 1 183 | echo. 184 | echo.Build finished. The text files are in %BUILDDIR%/text. 185 | goto end 186 | ) 187 | 188 | if "%1" == "man" ( 189 | %SPHINXBUILD% -b man %ALLSPHINXOPTS% %BUILDDIR%/man 190 | if errorlevel 1 exit /b 1 191 | echo. 192 | echo.Build finished. The manual pages are in %BUILDDIR%/man. 193 | goto end 194 | ) 195 | 196 | if "%1" == "texinfo" ( 197 | %SPHINXBUILD% -b texinfo %ALLSPHINXOPTS% %BUILDDIR%/texinfo 198 | if errorlevel 1 exit /b 1 199 | echo. 200 | echo.Build finished. The Texinfo files are in %BUILDDIR%/texinfo. 201 | goto end 202 | ) 203 | 204 | if "%1" == "gettext" ( 205 | %SPHINXBUILD% -b gettext %I18NSPHINXOPTS% %BUILDDIR%/locale 206 | if errorlevel 1 exit /b 1 207 | echo. 208 | echo.Build finished. The message catalogs are in %BUILDDIR%/locale. 209 | goto end 210 | ) 211 | 212 | if "%1" == "changes" ( 213 | %SPHINXBUILD% -b changes %ALLSPHINXOPTS% %BUILDDIR%/changes 214 | if errorlevel 1 exit /b 1 215 | echo. 216 | echo.The overview file is in %BUILDDIR%/changes. 217 | goto end 218 | ) 219 | 220 | if "%1" == "linkcheck" ( 221 | %SPHINXBUILD% -b linkcheck %ALLSPHINXOPTS% %BUILDDIR%/linkcheck 222 | if errorlevel 1 exit /b 1 223 | echo. 224 | echo.Link check complete; look for any errors in the above output ^ 225 | or in %BUILDDIR%/linkcheck/output.txt. 226 | goto end 227 | ) 228 | 229 | if "%1" == "doctest" ( 230 | %SPHINXBUILD% -b doctest %ALLSPHINXOPTS% %BUILDDIR%/doctest 231 | if errorlevel 1 exit /b 1 232 | echo. 233 | echo.Testing of doctests in the sources finished, look at the ^ 234 | results in %BUILDDIR%/doctest/output.txt. 235 | goto end 236 | ) 237 | 238 | if "%1" == "coverage" ( 239 | %SPHINXBUILD% -b coverage %ALLSPHINXOPTS% %BUILDDIR%/coverage 240 | if errorlevel 1 exit /b 1 241 | echo. 242 | echo.Testing of coverage in the sources finished, look at the ^ 243 | results in %BUILDDIR%/coverage/python.txt. 244 | goto end 245 | ) 246 | 247 | if "%1" == "xml" ( 248 | %SPHINXBUILD% -b xml %ALLSPHINXOPTS% %BUILDDIR%/xml 249 | if errorlevel 1 exit /b 1 250 | echo. 251 | echo.Build finished. The XML files are in %BUILDDIR%/xml. 252 | goto end 253 | ) 254 | 255 | if "%1" == "pseudoxml" ( 256 | %SPHINXBUILD% -b pseudoxml %ALLSPHINXOPTS% %BUILDDIR%/pseudoxml 257 | if errorlevel 1 exit /b 1 258 | echo. 259 | echo.Build finished. The pseudo-XML files are in %BUILDDIR%/pseudoxml. 260 | goto end 261 | ) 262 | 263 | :end 264 | -------------------------------------------------------------------------------- /autompc/sysid/model.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | import numpy as np 4 | 5 | from abc import ABC, abstractmethod 6 | from pdb import set_trace 7 | 8 | class ModelFactory(ABC): 9 | """ 10 | The ModelFactory creates and trains a System ID model and provides 11 | information about the model hyperparameters. 12 | """ 13 | def __init__(self, system, **kwargs): 14 | """ 15 | Parameters 16 | ---------- 17 | system : System 18 | System for which system ID model will be produced. 19 | """ 20 | self.system = system 21 | self.kwargs = kwargs 22 | 23 | def __call__(self, cfg, train_trajs, silent=False, skip_train_model=False): 24 | """ 25 | Returns a model trained for the given 26 | system and configuration. 27 | 28 | Parameters 29 | ---------- 30 | cfg : Configuration 31 | Configuration of model hyperparameters 32 | train_trajs : List of Trajectory objects 33 | Model training data set 34 | silent : bool 35 | Whether to produce output during training 36 | skip_train_model : bool 37 | Skip model training when True. 38 | """ 39 | model_args = cfg.get_dictionary() 40 | model_args.update(self.kwargs) 41 | model = self.Model(self.system, **model_args) 42 | model.factory = self 43 | if not skip_train_model: 44 | model.train(train_trajs, silent=silent) 45 | 46 | return model 47 | 48 | @abstractmethod 49 | def get_configuration_space(self): 50 | """ 51 | Returns the model configuration space. 52 | """ 53 | raise NotImplementedError 54 | 55 | class Model(ABC): 56 | def __init__(self, system): 57 | self.system = system 58 | 59 | @abstractmethod 60 | def traj_to_state(self, traj): 61 | """ 62 | Parameters 63 | ---------- 64 | traj : Trajectory 65 | State and control history up to present time 66 | Returns 67 | ------- 68 | state : numpy array of size self.state_dim 69 | Corresponding model state 70 | """ 71 | raise NotImplementedError 72 | 73 | @abstractmethod 74 | def update_state(self, state, new_ctrl, new_obs): 75 | """ 76 | Parameters 77 | ---------- 78 | state : numpy array of size self.state_dim 79 | Current model state 80 | new_ctrl : numpy array of size self.system.ctrl_dim 81 | New control input 82 | new_obs : numpy array of size self.system.obs_dim 83 | New observation 84 | Returns 85 | ------- 86 | state : numpy array of size self.state_dim 87 | Model state after observation and control 88 | """ 89 | raise NotImplementedError 90 | 91 | @abstractmethod 92 | def pred(self, state, ctrl): 93 | """ 94 | Run model prediction. 95 | 96 | Parameters 97 | ---------- 98 | state : Numpy array of size self.state_dim 99 | Model state at time t 100 | ctrl : Numpy array of size self.system.ctrl_dim 101 | Control applied at time t 102 | Returns 103 | ------- 104 | state : Numpy array of size self.state_dim 105 | Predicted model state at time t+1 106 | """ 107 | raise NotImplementedError 108 | 109 | def pred_batch(self, states, ctrls): 110 | """ 111 | Run batch model predictions. Depending on the model, this can 112 | be much faster than repeatedly calling pred. 113 | 114 | Parameters 115 | ---------- 116 | state : Numpy array of size (N, self.state_dim) 117 | N model input states 118 | ctrl : Numpy array of size (N, self.system.ctrl_dim) 119 | N controls 120 | Returns 121 | ------- 122 | state : Numpy array of size (N, self.state_dim) 123 | N predicted states 124 | """ 125 | n = self.state_dim 126 | m = states.shape[0] 127 | out = np.empty((m, n)) 128 | for i in range(m): 129 | out[i,:] = self.pred(states[i,:], ctrls[i,:]) 130 | return out 131 | 132 | def pred_diff(self, state, ctrl): 133 | """ 134 | Run model prediction and compute gradients. 135 | 136 | Parameters 137 | ---------- 138 | state : Numpy array of size self.state_dim 139 | Model state at time t 140 | ctrl : Numpy array of size self.system.ctrl_dim 141 | Control at time t 142 | Returns 143 | ------- 144 | state : Numpy array of size self.state_dim 145 | Predicted model state at time t+1 146 | state_jac : Numpy array of shape (self.state_dim, 147 | self.state_dim) 148 | Gradient of predicted model state wrt to state 149 | ctrl_jac : Numpy array of shape (self.state_dim, 150 | self.ctrl_dim) 151 | Gradient of predicted model state wrt to ctrl 152 | """ 153 | raise NotImplementedError 154 | 155 | def pred_diff_batch(self, states, ctrls): 156 | """ 157 | Run model prediction and compute gradients in batch. 158 | 159 | Parameters 160 | ---------- 161 | state : Numpy array of shape (N, self.state_dim) 162 | N input model states 163 | ctrl : Numpy array of size (N, self.system.ctrl_dim) 164 | N input controls 165 | Returns 166 | ------- 167 | state : Numpy array of size (N, self.state_dim) 168 | N predicted model states 169 | state_jac : Numpy array of shape (N, self.state_dim, 170 | self.state_dim) 171 | Gradient of predicted model states wrt to state 172 | ctrl_jac : Numpy array of shape (N, self.state_dim, 173 | self.ctrl_dim) 174 | Gradient of predicted model states wrt to ctrl 175 | """ 176 | n = self.state_dim 177 | m = states.shape[0] 178 | out = np.empty((m, n)) 179 | state_jacs = np.empty((m, n, n)) 180 | ctrl_jacs = np.empty((m, n, self.system.ctrl_dim)) 181 | for i in range(m): 182 | out[i,:], state_jacs[i,:,:], ctrl_jacs[i,:,:] = \ 183 | self.pred_diff(states[i,:], ctrls[i,:]) 184 | return out, state_jacs, ctrl_jacs 185 | 186 | 187 | def to_linear(self): 188 | """ 189 | Returns: (A, B, state_func, cost_func) 190 | A, B -- Linear system matrices as Numpy arrays. 191 | Only implemented for linear models. 192 | """ 193 | raise NotImplementedError 194 | 195 | def train(self, trajs, silent=False): 196 | """ 197 | Parameters 198 | ---------- 199 | trajs : List of pairs (xs, us) 200 | Training set of trajectories 201 | silent : bool 202 | Silence progress bar output 203 | Only implemented for trainable models. 204 | """ 205 | raise NotImplementedError 206 | 207 | def get_parameters(self): 208 | """ 209 | Returns a dict containing trained model parameters. 210 | 211 | Only implemented for trainable models. 212 | """ 213 | raise NotImplementedError 214 | 215 | def set_parameters(self, params): 216 | """ 217 | Sets trainable model parameters from dict. 218 | 219 | Only implemented for trainable parameters. 220 | """ 221 | raise NotImplementedError 222 | 223 | @property 224 | @abstractmethod 225 | def state_dim(self): 226 | """ 227 | Returns the size of the model state 228 | """ 229 | raise NotImplementedError 230 | 231 | 232 | @property 233 | def is_linear(self): 234 | """ 235 | Returns true for linear models 236 | """ 237 | return not self.to_linear.__func__ is Model.to_linear 238 | 239 | @property 240 | def is_diff(self): 241 | """ 242 | Returns true for differentiable models. 243 | """ 244 | return not self.pred_diff.__func__ is Model.pred_diff 245 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | PAPER = 8 | BUILDDIR = _build 9 | 10 | # User-friendly check for sphinx-build 11 | ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) 12 | $(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/) 13 | endif 14 | 15 | # Internal variables. 16 | PAPEROPT_a4 = -D latex_paper_size=a4 17 | PAPEROPT_letter = -D latex_paper_size=letter 18 | ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 19 | # the i18n builder cannot share the environment and doctrees with the others 20 | I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . 21 | 22 | .PHONY: help 23 | help: 24 | @echo "Please use \`make ' where is one of" 25 | @echo " html to make standalone HTML files" 26 | @echo " dirhtml to make HTML files named index.html in directories" 27 | @echo " singlehtml to make a single large HTML file" 28 | @echo " pickle to make pickle files" 29 | @echo " json to make JSON files" 30 | @echo " htmlhelp to make HTML files and a HTML help project" 31 | @echo " qthelp to make HTML files and a qthelp project" 32 | @echo " applehelp to make an Apple Help Book" 33 | @echo " devhelp to make HTML files and a Devhelp project" 34 | @echo " epub to make an epub" 35 | @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" 36 | @echo " latexpdf to make LaTeX files and run them through pdflatex" 37 | @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" 38 | @echo " text to make text files" 39 | @echo " man to make manual pages" 40 | @echo " texinfo to make Texinfo files" 41 | @echo " info to make Texinfo files and run them through makeinfo" 42 | @echo " gettext to make PO message catalogs" 43 | @echo " changes to make an overview of all changed/added/deprecated items" 44 | @echo " xml to make Docutils-native XML files" 45 | @echo " pseudoxml to make pseudoxml-XML files for display purposes" 46 | @echo " linkcheck to check all external links for integrity" 47 | @echo " doctest to run all doctests embedded in the documentation (if enabled)" 48 | @echo " coverage to run coverage check of the documentation (if enabled)" 49 | 50 | .PHONY: clean 51 | clean: 52 | rm -rf $(BUILDDIR)/* 53 | 54 | .PHONY: html 55 | html: 56 | $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html 57 | @echo 58 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." 59 | 60 | .PHONY: dirhtml 61 | dirhtml: 62 | $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml 63 | @echo 64 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." 65 | 66 | .PHONY: singlehtml 67 | singlehtml: 68 | $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml 69 | @echo 70 | @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." 71 | 72 | .PHONY: pickle 73 | pickle: 74 | $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle 75 | @echo 76 | @echo "Build finished; now you can process the pickle files." 77 | 78 | .PHONY: json 79 | json: 80 | $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json 81 | @echo 82 | @echo "Build finished; now you can process the JSON files." 83 | 84 | .PHONY: htmlhelp 85 | htmlhelp: 86 | $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp 87 | @echo 88 | @echo "Build finished; now you can run HTML Help Workshop with the" \ 89 | ".hhp project file in $(BUILDDIR)/htmlhelp." 90 | 91 | .PHONY: qthelp 92 | qthelp: 93 | $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp 94 | @echo 95 | @echo "Build finished; now you can run "qcollectiongenerator" with the" \ 96 | ".qhcp project file in $(BUILDDIR)/qthelp, like this:" 97 | @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/AutoMPC.qhcp" 98 | @echo "To view the help file:" 99 | @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/AutoMPC.qhc" 100 | 101 | .PHONY: applehelp 102 | applehelp: 103 | $(SPHINXBUILD) -b applehelp $(ALLSPHINXOPTS) $(BUILDDIR)/applehelp 104 | @echo 105 | @echo "Build finished. The help book is in $(BUILDDIR)/applehelp." 106 | @echo "N.B. You won't be able to view it unless you put it in" \ 107 | "~/Library/Documentation/Help or install it in your application" \ 108 | "bundle." 109 | 110 | .PHONY: devhelp 111 | devhelp: 112 | $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp 113 | @echo 114 | @echo "Build finished." 115 | @echo "To view the help file:" 116 | @echo "# mkdir -p $$HOME/.local/share/devhelp/AutoMPC" 117 | @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/AutoMPC" 118 | @echo "# devhelp" 119 | 120 | .PHONY: epub 121 | epub: 122 | $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub 123 | @echo 124 | @echo "Build finished. The epub file is in $(BUILDDIR)/epub." 125 | 126 | .PHONY: latex 127 | latex: 128 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 129 | @echo 130 | @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." 131 | @echo "Run \`make' in that directory to run these through (pdf)latex" \ 132 | "(use \`make latexpdf' here to do that automatically)." 133 | 134 | .PHONY: latexpdf 135 | latexpdf: 136 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 137 | @echo "Running LaTeX files through pdflatex..." 138 | $(MAKE) -C $(BUILDDIR)/latex all-pdf 139 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 140 | 141 | .PHONY: latexpdfja 142 | latexpdfja: 143 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 144 | @echo "Running LaTeX files through platex and dvipdfmx..." 145 | $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja 146 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 147 | 148 | .PHONY: text 149 | text: 150 | $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text 151 | @echo 152 | @echo "Build finished. The text files are in $(BUILDDIR)/text." 153 | 154 | .PHONY: man 155 | man: 156 | $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man 157 | @echo 158 | @echo "Build finished. The manual pages are in $(BUILDDIR)/man." 159 | 160 | .PHONY: texinfo 161 | texinfo: 162 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 163 | @echo 164 | @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." 165 | @echo "Run \`make' in that directory to run these through makeinfo" \ 166 | "(use \`make info' here to do that automatically)." 167 | 168 | .PHONY: info 169 | info: 170 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 171 | @echo "Running Texinfo files through makeinfo..." 172 | make -C $(BUILDDIR)/texinfo info 173 | @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." 174 | 175 | .PHONY: gettext 176 | gettext: 177 | $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale 178 | @echo 179 | @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." 180 | 181 | .PHONY: changes 182 | changes: 183 | $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes 184 | @echo 185 | @echo "The overview file is in $(BUILDDIR)/changes." 186 | 187 | .PHONY: linkcheck 188 | linkcheck: 189 | $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck 190 | @echo 191 | @echo "Link check complete; look for any errors in the above output " \ 192 | "or in $(BUILDDIR)/linkcheck/output.txt." 193 | 194 | .PHONY: doctest 195 | doctest: 196 | $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest 197 | @echo "Testing of doctests in the sources finished, look at the " \ 198 | "results in $(BUILDDIR)/doctest/output.txt." 199 | 200 | .PHONY: coverage 201 | coverage: 202 | $(SPHINXBUILD) -b coverage $(ALLSPHINXOPTS) $(BUILDDIR)/coverage 203 | @echo "Testing of coverage in the sources finished, look at the " \ 204 | "results in $(BUILDDIR)/coverage/python.txt." 205 | 206 | .PHONY: xml 207 | xml: 208 | $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml 209 | @echo 210 | @echo "Build finished. The XML files are in $(BUILDDIR)/xml." 211 | 212 | .PHONY: pseudoxml 213 | pseudoxml: 214 | $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml 215 | @echo 216 | @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." 217 | -------------------------------------------------------------------------------- /autompc/control/lqr.py: -------------------------------------------------------------------------------- 1 | # Created by William Edwards (wre2@illinois.edu) 2 | 3 | from pdb import set_trace 4 | 5 | import numpy as np 6 | import numpy.linalg as la 7 | 8 | from ConfigSpace import ConfigurationSpace 9 | from ConfigSpace.hyperparameters import (UniformIntegerHyperparameter, 10 | CategoricalHyperparameter) 11 | import ConfigSpace.conditions as CSC 12 | 13 | from .controller import Controller, ControllerFactory 14 | 15 | def _dynamic_ricatti_equation(A, B, Q, R, N, Pk): 16 | return (A.T @ Pk @ A 17 | - (A.T @ Pk @ B + N) 18 | @ la.inv(R + B.T @ Pk @ B) 19 | @ (B.T @ Pk @ A + N.T) 20 | + Q) 21 | 22 | def _inf_horz_dt_lqr(A, B, Q, R, N, threshold=1e-3): 23 | P1 = Q 24 | P2 = _dynamic_ricatti_equation(A, B, Q, R, N, P1) 25 | Pdiff = np.abs(P1 - P2) 26 | while Pdiff.max() > threshold: 27 | P1 = P2 28 | P2 = _dynamic_ricatti_equation(A, B, Q, R, N, P1) 29 | Pdiff = np.abs(P1 - P2) 30 | 31 | K = -la.inv(R + B.T @ P2 @ B) @ B.T @ P2 @ A 32 | 33 | return K 34 | 35 | def _finite_horz_dt_lqr(A, B, Q, R, N, F, horizon): 36 | P1 = F 37 | P2 = _dynamic_ricatti_equation(A, B, Q, R, N, P1) 38 | for _ in range(horizon): 39 | P1 = P2 40 | P2 = _dynamic_ricatti_equation(A, B, Q, R, N, P1) 41 | Pdiff = np.abs(P1 - P2) 42 | 43 | K = -la.inv(R + B.T @ P2 @ B) @ B.T @ P2 @ A 44 | print("P2=") 45 | print(P2) 46 | 47 | return K 48 | 49 | #class InfiniteHorizonLQR(Controller): 50 | # def __init__(self, system, model, Q, R): 51 | # if not model.is_linear: 52 | # raise ValueError("Linear model required.") 53 | # super().__init__(system, model) 54 | # A, B, state_func, cost_func = model.to_linear() 55 | # Qp, Rp = cost_func(Q, R) 56 | # N = np.zeros((A.shape[0], B.shape[1])) 57 | # self.K = _inf_horz_dt_lqr(A, B, Qp, Rp, N) 58 | # self.state_func = state_func 59 | # self.Qp, self.Rp = Qp, Rp 60 | # 61 | # def run(self, traj, latent=None): 62 | # # Implement control logic here 63 | # u = self.K @ self.state_func(traj) 64 | # 65 | # return u, None 66 | 67 | # class LQRFactory(ControllerFactory): 68 | # """ 69 | # Docs 70 | # 71 | # Hyperparameters: 72 | # 73 | # - *horizon_type* (Type: str, Choices: ["finite", "infinite"], Default: "finite"): Whether horizon is finite or infinite. 74 | # - *horizon* (Type: int, Low: 1, High: 1000, Default: 10): Length of control horizon. (Conditioned on horizon_type="finite"). 75 | # """ 76 | # def __init__(self, *args, **kwargs): 77 | # super().__init__(*args, **kwargs) 78 | # self.name = "InfiniteHorizonLQR" 79 | # 80 | # def get_configuration_space(self): 81 | # cs = ConfigurationSpace() 82 | # horizon_type = CategoricalHyperparameter("horizon_type", choices=["finite", "infinite"], default="finite") 83 | # horizon = UniformIntegerHyperparameter(name="horizon_cond", 84 | # lower=1, upper=1000, default_value=10) 85 | # horizon_cond = InCondition(child=horizon, parent=horizon_type, values=["finite"]) 86 | # cs.add_hyperparameters([horizon, horizon_type]) 87 | # cs.add_condition(horizon_cond) 88 | # return cs 89 | # 90 | # def __call__(self, cfg, task, model): 91 | # if cfg["horizon_type"] == "finite": 92 | # controller = FiniteHorizonLQR(self.system, task, model, horizon = cfg["horizon"]) 93 | # else: 94 | # controller = InfiniteHorizonLQR(self.system, task, model) 95 | 96 | class InfiniteHorizonLQR(Controller): 97 | def __init__(self, system, task, model): 98 | super().__init__(system, task, model) 99 | A, B = model.to_linear() 100 | state_dim = model.state_dim 101 | Q, R, F = task.get_cost().get_cost_matrices() 102 | Qp = np.zeros((state_dim, state_dim)) 103 | Qp[:Q.shape[0], :Q.shape[1]] = Q 104 | X, L, K = dare(A, B, Qp, R) 105 | self.K = K 106 | self.Qp, self.Rp = Qp, R 107 | self.model = model 108 | 109 | @property 110 | def state_dim(self): 111 | return self.model.state_dim + self.system.ctrl_dim 112 | 113 | @staticmethod 114 | def is_compatible(system, task, model): 115 | return (model.is_linear 116 | and task.is_cost_quad() 117 | and not task.are_obs_bounded() 118 | and not task.are_ctrl_bounded() 119 | and not task.eq_cons_present() 120 | and not task.ineq_cons_present()) 121 | 122 | def traj_to_state(self, traj): 123 | return np.concatenate([self.model.traj_to_state(traj), 124 | traj[-1].ctrl]) 125 | 126 | def run(self, state, new_obs): 127 | # Implement control logic here 128 | modelstate = self.model.update_state(state[:-self.system.ctrl_dim], 129 | state[-self.system.ctrl_dim:], new_obs) 130 | u = np.array(self.K @ modelstate).flatten() 131 | print("state={}".format(state)) 132 | print("u={}".format(u)) 133 | print("state_cost={}".format(modelstate.T @ self.Qp @ modelstate)) 134 | statenew = np.concatenate([modelstate, u]) 135 | 136 | return u, statenew 137 | 138 | class FiniteHorizonLQR(Controller): 139 | def __init__(self, system, task, model, horizon): 140 | super().__init__(system, task, model) 141 | A, B = model.to_linear() 142 | N = np.zeros((A.shape[0], B.shape[1])) 143 | self.horizon = horizon 144 | state_dim = model.state_dim 145 | #Q, R, F = task.get_quad_cost() 146 | Q, R, F = task.get_cost().get_cost_matrices() 147 | Qp = np.zeros((state_dim, state_dim)) 148 | Qp[:Q.shape[0], :Q.shape[1]] = Q 149 | Fp = np.zeros((state_dim, state_dim)) 150 | Fp[:F.shape[0], :F.shape[1]] = F 151 | self.K = _finite_horz_dt_lqr(A, B, Qp, R, N, Fp, horizon) 152 | self.Qp, self.Rp = Qp, R 153 | self.model = model 154 | self.umin = task.get_ctrl_bounds()[:,0] 155 | self.umax = task.get_ctrl_bounds()[:,1] 156 | 157 | @property 158 | def state_dim(self): 159 | return self.model.state_dim + self.system.ctrl_dim 160 | 161 | @staticmethod 162 | def is_compatible(system, task, model): 163 | return (model.is_linear 164 | and task.is_cost_quad() 165 | and not task.are_obs_bounded() 166 | #and not task.are_ctrl_bounded() 167 | and not task.eq_cons_present() 168 | and not task.ineq_cons_present()) 169 | 170 | def traj_to_state(self, traj): 171 | return np.concatenate([self.model.traj_to_state(traj), 172 | traj[-1].ctrl]) 173 | 174 | def run(self, state, new_obs): 175 | # Implement control logic here 176 | modelstate = self.model.update_state(state[:-self.system.ctrl_dim], 177 | state[-self.system.ctrl_dim:], new_obs) 178 | x0 = self.task.get_cost().get_goal() 179 | if x0.size < modelstate.size: 180 | state0 = np.zeros(modelstate.size) 181 | state0[:x0.size] = x0 182 | else: 183 | state0 = x0 184 | u = self.K @ (modelstate - state0) 185 | u = np.minimum(u, self.umax) 186 | u = np.maximum(u, self.umin) 187 | #print("state={}".format(state)) 188 | #print("u={}".format(u)) 189 | #print("state_cost={}".format(modelstate.T @ self.Qp @ modelstate)) 190 | statenew = np.concatenate([modelstate, u]) 191 | 192 | return u, statenew 193 | 194 | class LQRFactory(ControllerFactory): 195 | """ 196 | Linear Quadratic Regulator (LQR) is some classical results from linear system theory and optimal control theory. 197 | It applies to linear system with quadratic cost function with respect to both state and control. 198 | It is proven that the optimal control policy is linear, i.e. :math:`u=-Kx` where :math:`x` is system state, :math:`K` is gain matrix, and :math:`u` is the control. 199 | The feedback :math:`K` is computed by solving Ricatti equations. 200 | For more details refer to these slides_ . 201 | 202 | .. _slides: https://katefvision.github.io/katefSlides/RECITATIONtrajectoryoptimization_katef.pdf 203 | 204 | Hyperparameters: 205 | 206 | - *finite_horizon* (Type: str, Choices: ["true", "false"], Default: "finite"): Whether horizon is finite or infinite. 207 | - *horizon* (Type: int, Low: 1, High: 1000, Default: 10): Length of control horizon. (Conditioned on finite_horizon="true"). 208 | """ 209 | def __init__(self, *args, **kwargs): 210 | super().__init__(*args, **kwargs) 211 | self.Controller = LQR 212 | self.name = "LQR" 213 | 214 | def get_configuration_space(self): 215 | cs = ConfigurationSpace() 216 | finite_horizon = CategoricalHyperparameter(name="finite_horizon", 217 | choices=["true", "false"], default_value="true") 218 | horizon = UniformIntegerHyperparameter(name="horizon", 219 | lower=1, upper=1000, default_value=10) 220 | use_horizon = CSC.InCondition(child=horizon, parent=finite_horizon, 221 | values=["true"]) 222 | cs.add_hyperparameters([horizon, finite_horizon]) 223 | cs.add_condition(use_horizon) 224 | return cs 225 | 226 | class LQR(Controller): 227 | def __init__(self, system, task, model, finite_horizon, horizon=None): 228 | super().__init__(system, task, model) 229 | if not isinstance(finite_horizon, bool): 230 | finite_horizon = True if finite_horizon == "true" else False 231 | if finite_horizon: 232 | self._controller = FiniteHorizonLQR(system, task, model, horizon) 233 | else: 234 | self._controller = InfiniteHorizonLQR(system, task, model) 235 | 236 | @property 237 | def state_dim(self): 238 | return self._controller.state_dim 239 | 240 | @staticmethod 241 | def is_compatible(system, task, model): 242 | return (model.is_linear 243 | and task.is_cost_quad() 244 | and not task.are_obs_bounded() 245 | #and not task.are_ctrl_bounded() 246 | and not task.eq_cons_present() 247 | and not task.ineq_cons_present()) 248 | 249 | def traj_to_state(self, traj): 250 | return self._controller.traj_to_state(traj) 251 | 252 | def run(self, state, new_obs): 253 | return self._controller.run(state, new_obs) 254 | --------------------------------------------------------------------------------