├── .gitignore ├── .gitmodules ├── LICENSE ├── examples ├── example.py ├── example2.py └── utils.py ├── pyzpc ├── __init__.py ├── utils.py ├── zpc.py └── zpc_simplified.py └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | dist/ 3 | /*.egg-info 4 | build/ 5 | .ipynb_checkpoints 6 | __pycache__/ -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rssalessio/PyZPC/dad3640304088c706a471ac1390685ad68ee2b2d/.gitmodules -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Alessio Russo 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /examples/example.py: -------------------------------------------------------------------------------- 1 | # To run this example you also need to install matplotlib 2 | import numpy as np 3 | import scipy.signal as scipysig 4 | import cvxpy as cp 5 | import matplotlib.pyplot as plt 6 | 7 | from typing import List 8 | from cvxpy.expressions.expression import Expression 9 | from cvxpy.constraints.constraint import Constraint 10 | from pyzpc import ZPC, Data, SystemZonotopes 11 | from utils import generate_trajectories 12 | from pyzonotope import Zonotope 13 | 14 | # Define the loss function 15 | def loss_callback(u: cp.Variable, y: cp.Variable) -> Expression: 16 | horizon, M, P = u.shape[0], u.shape[1], y.shape[1] 17 | ref = np.array([[1,0,0,0]]*horizon) 18 | 19 | # Sum_t ||y_t - r_t||^2 20 | cost = 0 21 | for i in range(horizon): 22 | cost += 100*cp.norm(y[i,0] - 1) 23 | return cost 24 | 25 | # Define additional constraints 26 | def constraints_callback(u: cp.Variable, y: cp.Variable) -> List[Constraint]: 27 | horizon, M, P = u.shape[0], u.shape[1], y.shape[1] 28 | # Define a list of additional input/output constraints 29 | return [] 30 | 31 | 32 | # Plant 33 | # In this example we consider the three-pulley 34 | # system analyzed in the original VRFT paper: 35 | # 36 | # "Virtual reference feedback tuning: 37 | # a direct method for the design offeedback controllers" 38 | # -- Campi et al. 2003 39 | 40 | dt = 0.05 41 | num = [0.28261, 0.50666] 42 | den = [1, -1.41833, 1.58939, -1.31608, 0.88642] 43 | sys = scipysig.TransferFunction(num, den, dt=dt).to_ss() 44 | dim_x, dim_u = sys.B.shape 45 | 46 | 47 | # Define zonotopes and generate data 48 | X0 = Zonotope([0] * dim_x, 0. * np.diag([1] * dim_x)) 49 | U = Zonotope([1] * dim_u, 3 * np.diag([1] * dim_u)) 50 | W = Zonotope([0] * dim_x, 0.005 * np.ones((dim_x, 1))) 51 | V = Zonotope([0] * dim_x, 0.002 * np.ones((dim_x, 1))) 52 | Y = Zonotope([1] * dim_x, np.diag(2*np.ones(dim_x))) 53 | AV = V * sys.A 54 | zonotopes = SystemZonotopes(X0, U, Y, W, V, AV) 55 | 56 | num_trajectories = 5 57 | num_steps_per_trajectory = 200 58 | horizon = 1 59 | 60 | data = generate_trajectories(sys, X0, U, W, V, num_trajectories, num_steps_per_trajectory) 61 | 62 | # Build DPC 63 | zpc = ZPC(data) 64 | 65 | x = X0.sample().flatten() 66 | 67 | trajectory = [x] 68 | problem = zpc.build_problem(zonotopes, horizon, loss_callback, constraints_callback) 69 | for n in range(100): 70 | # print(f'Solving step {n}') 71 | 72 | result, info = zpc.solve(x, verbose=False,warm_start=True) 73 | print(f'[iteration {n}] su: {problem.variables.su.value} - sl: {problem.variables.sl.value}') 74 | u = info['u_optimal'] 75 | z = sys.A @ x + np.squeeze(sys.B *u[0]) + W.sample() 76 | 77 | # We assume C = I 78 | x = (z + V.sample()).flatten() 79 | trajectory.append(x) 80 | 81 | trajectory = np.array(trajectory) 82 | for i in range(dim_x): 83 | plt.plot(trajectory[:,i], label=f'x{i}') 84 | 85 | plt.grid() 86 | plt.legend() 87 | plt.show() 88 | -------------------------------------------------------------------------------- /examples/example2.py: -------------------------------------------------------------------------------- 1 | # To run this example you also need to install matplotlib 2 | import numpy as np 3 | import scipy.signal as scipysig 4 | import cvxpy as cp 5 | import matplotlib.pyplot as plt 6 | 7 | from typing import List 8 | from cvxpy.expressions.expression import Expression 9 | from cvxpy.constraints.constraint import Constraint 10 | from pyzpc import ZPC, Data, SystemZonotopes 11 | from utils import generate_trajectories 12 | from pyzonotope import Zonotope 13 | 14 | 15 | 16 | A = np.array( 17 | [[-1, -4, 0, 0, 0], 18 | [4, -1, 0, 0, 0], 19 | [0, 0, -3, 1, 0], 20 | [0, 0, -1, -3, 0], 21 | [0, 0, 0, 0, -2]]) 22 | B = np.ones((5, 1)) 23 | C = np.array([1, 0, 0, 0, 0]) 24 | D = np.array([0]) 25 | 26 | dim_x = A.shape[0] 27 | dim_u = 1 28 | dt = 0.05 29 | A,B,C,D,_ = scipysig.cont2discrete(system=(A,B,C,D), dt = dt) 30 | 31 | uref = np.array([8]) 32 | xref = (np.linalg.inv(np.eye(dim_x) - A) @ B @ uref).flatten() 33 | 34 | 35 | # Define the loss function 36 | def loss_callback(u: cp.Variable, y: cp.Variable) -> Expression: 37 | horizon, M, P = u.shape[0], u.shape[1], y.shape[1] 38 | ref = np.array([[1,0,0,0]]*horizon) 39 | 40 | # Sum_t ||y_t - r_t||^2 41 | cost = 0 42 | for i in range(horizon): 43 | cost += 1e3 * cp.norm(y[i,:] - xref, p =2) + 1e-3 * cp.norm(u[i,:] - uref,p=2) 44 | return cost 45 | 46 | # Define additional constraints 47 | def constraints_callback(u: cp.Variable, y: cp.Variable) -> List[Constraint]: 48 | horizon, M, P = u.shape[0], u.shape[1], y.shape[1] 49 | # Define a list of additional input/output constraints 50 | return [] 51 | 52 | 53 | # Define zonotopes and generate data 54 | X0 = Zonotope([-2, 4, 3, -2.5, 5.5], 1 * np.diag([1] * dim_x)) 55 | U = Zonotope([1] * dim_u, 10 * np.diag([1] * dim_u)) 56 | W = Zonotope([0] * dim_x, 0.005 * np.ones((dim_x, 1))) 57 | V = Zonotope([0] * dim_x, 0.002 * np.ones((dim_x, 1))) 58 | Y = Zonotope([1] * dim_x, 15*np.diag(np.ones(dim_x))) 59 | AV = V * A 60 | zonotopes = SystemZonotopes(X0, U, Y, W, V, AV) 61 | 62 | num_trajectories = 5 63 | num_steps_per_trajectory = 200 64 | horizon = 2 65 | 66 | data = generate_trajectories(scipysig.StateSpace(A,B,C,D), X0, U, W, V, num_trajectories, num_steps_per_trajectory) 67 | 68 | # Build DPC 69 | zpc = ZPC(data) 70 | 71 | x = X0.sample().flatten() 72 | 73 | trajectory = [x] 74 | problem = zpc.build_problem(zonotopes, horizon, loss_callback, constraints_callback) 75 | for n in range(100): 76 | print(f'Solving step {n}') 77 | 78 | result, info = zpc.solve(x, verbose=True,warm_start=True) 79 | u = info['u_optimal'] 80 | z = A @ x + np.squeeze(B *u[0]) + W.sample() 81 | 82 | # We assume C = I 83 | x = (z + V.sample()).flatten() 84 | trajectory.append(x) 85 | 86 | trajectory = np.array(trajectory) 87 | 88 | for i in range(dim_x): 89 | plt.plot(trajectory[:,i], label=f'x{i}') 90 | 91 | plt.grid() 92 | plt.legend() 93 | plt.show() 94 | -------------------------------------------------------------------------------- /examples/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.signal import StateSpace 3 | from pyzonotope import Zonotope 4 | from pyzpc import Data 5 | 6 | def generate_trajectories( 7 | sys: StateSpace, 8 | X0: Zonotope, 9 | U: Zonotope, 10 | W: Zonotope, 11 | V: Zonotope, 12 | num_trajectories: int, 13 | num_steps: int) -> Data: 14 | """ 15 | Generates trajectories from the system. We assume full state 16 | measurement (i.e., C=i) 17 | 18 | X0,U,W,V are respectively the zonotopes of the: 19 | - initial condition 20 | - control signal 21 | - Process noise 22 | - Measurement noise 23 | 24 | Returns a data object 25 | """ 26 | 27 | dim_x, dim_u = sys.B.shape 28 | total_samples = num_steps * num_trajectories 29 | u = U.sample(total_samples).reshape((num_trajectories, num_steps, dim_u)) 30 | 31 | # Simulate system 32 | X = np.zeros((num_trajectories, num_steps, dim_x)) 33 | Y = np.zeros((num_trajectories, num_steps, dim_x)) 34 | for j in range(num_trajectories): 35 | X[j, 0, :] = X0.sample() 36 | for i in range(1, num_steps): 37 | X[j, i, :] = sys.A @ X[j, i - 1, :] + np.squeeze(sys.B * u[j, i - 1]) + W.sample() 38 | 39 | # We assume C = I 40 | Y[j, i, :] = X[j, i, :] + V.sample() 41 | 42 | y = np.reshape(Y, (num_steps * num_trajectories, dim_x)) 43 | u = np.reshape(u, (num_steps * num_trajectories, dim_u)) 44 | 45 | return Data(u, y) 46 | -------------------------------------------------------------------------------- /pyzpc/__init__.py: -------------------------------------------------------------------------------- 1 | from .zpc import ZPC 2 | from .utils import * 3 | 4 | __author__ = 'Alessio Russo - alessior@kth.se' 5 | __version__ = '0.0.1' 6 | __url__ = 'https://github.com/rssalessio/PyZPC' 7 | __info__ = { 8 | 'version': __version__, 9 | 'author': __author__, 10 | 'url': __url__ 11 | } -------------------------------------------------------------------------------- /pyzpc/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from typing import NamedTuple, Tuple, Optional, List, Union 3 | from cvxpy import Expression, Variable, Problem, Parameter 4 | from cvxpy.constraints.constraint import Constraint 5 | from pyzonotope import Zonotope, CVXZonotope 6 | 7 | class OptimizationProblemVariables(NamedTuple): 8 | """ 9 | Class used to store all the variables used in the optimization 10 | problem 11 | """ 12 | y0: Parameter 13 | u: Variable 14 | y: Variable 15 | su: Variable 16 | sl: Variable 17 | beta_u: Variable 18 | 19 | class OptimizationProblem(NamedTuple): 20 | """ 21 | Class used to store the elements an optimization problem 22 | :param problem_variables: variables of the opt. problem 23 | :param constraints: constraints of the problem 24 | :param objective_function: objective function 25 | :param problem: optimization problem object 26 | """ 27 | variables: OptimizationProblemVariables 28 | constraints: List[Constraint] 29 | objective_function: Expression 30 | problem: Problem 31 | reachable_set: List[CVXZonotope] 32 | 33 | 34 | class Data(NamedTuple): 35 | """ 36 | Tuple that contains input/output data 37 | :param u: input data 38 | :param y: output data 39 | """ 40 | u: np.ndarray 41 | y: np.ndarray 42 | 43 | 44 | class DataDrivenDataset(NamedTuple): 45 | """ 46 | Tuple that contains input/output data splitted 47 | according to Y+, Y- and U- (resp. Yp, Ym, Um). 48 | See also section 2.3 in https://arxiv.org/pdf/2103.14110.pdf 49 | """ 50 | Yp: np.ndarray 51 | Ym: np.ndarray 52 | Um: np.ndarray 53 | 54 | 55 | class SystemZonotopes(NamedTuple): 56 | """ 57 | Tuple that contains the zonotopes of the system 58 | 59 | :param X0: initial condition zonotope 60 | :param U: Input zonotope 61 | :param Y: Output zonotope 62 | :param W: process noise zonotope 63 | :param V: measurement noise zonotope 64 | :param Av: Onestep propagation zonotope 65 | """ 66 | X0: Zonotope 67 | U: Zonotope 68 | Y: Zonotope 69 | W: Zonotope 70 | V: Zonotope 71 | Av: Zonotope 72 | 73 | 74 | -------------------------------------------------------------------------------- /pyzpc/zpc.py: -------------------------------------------------------------------------------- 1 | from cmath import isinf 2 | from re import S 3 | import numpy as np 4 | import cvxpy as cp 5 | from typing import Tuple, Callable, List, Optional, Union, Dict 6 | from cvxpy.expressions.expression import Expression 7 | from cvxpy.constraints.constraint import Constraint 8 | from pyzonotope import ( 9 | concatenate_zonotope, 10 | MatrixZonotope, 11 | Zonotope, 12 | CVXZonotope) 13 | 14 | from pydatadrivenreachability import compute_IO_LTI_matrix_zonotope 15 | 16 | from pyzpc.utils import ( 17 | Data, 18 | DataDrivenDataset, 19 | SystemZonotopes, 20 | OptimizationProblem, 21 | OptimizationProblemVariables) 22 | import sys 23 | #sys.setrecursionlimit(10000) 24 | 25 | 26 | class ZPC(object): 27 | optimization_problem: Union[OptimizationProblem,None] = None 28 | dataset: DataDrivenDataset 29 | zonotopes: SystemZonotopes 30 | Msigma: MatrixZonotope 31 | 32 | def __init__(self, data: Data): 33 | """ 34 | Solves the ZPC optimization problem 35 | See also https://arxiv.org/pdf/2103.14110.pdf 36 | 37 | :param data: A tuple of input/output data. Data should have shape TxM 38 | where T is the batch size and M is the number of features 39 | """ 40 | self.update_identification_data(data) 41 | 42 | @property 43 | def num_samples(self) -> int: 44 | """ Return the number of samples used to estimate the Matrix Zonotope Msigma """ 45 | return self.dataset.Um.shape[0] + 1 46 | 47 | @property 48 | def dim_u(self) -> int: 49 | """ Return the dimensionality of u (the control signal) """ 50 | return self.dataset.Um.shape[1] 51 | 52 | @property 53 | def dim_y(self) -> int: 54 | """ Return the dimensionality of y (the output signal) """ 55 | return self.dataset.Yp.shape[1] 56 | 57 | def update_identification_data(self, data: Data): 58 | """ 59 | Update identification data matrices of ZPC. You need to rebuild the optimization problem 60 | after calling this funciton. 61 | 62 | :param data: A tuple of input/output data. Data should have shape TxM 63 | where T is the batch size and M is the number of features 64 | """ 65 | assert len(data.u.shape) == 2, \ 66 | "Data needs to be shaped as a TxM matrix (T is the number of samples and M is the number of features)" 67 | assert len(data.y.shape) == 2, \ 68 | "Data needs to be shaped as a TxM matrix (T is the number of samples and M is the number of features)" 69 | assert data.y.shape[0] == data.u.shape[0], \ 70 | "Input/output data must have the same length" 71 | 72 | Ym = data.y[:-1] 73 | Yp = data.y[1:] 74 | Um = data.u[:-1] 75 | self.dataset = DataDrivenDataset(Yp, Ym, Um) 76 | self.optimization_problem = None 77 | 78 | def _build_zonotopes(self, zonotopes: SystemZonotopes): 79 | """ 80 | [Private method] Do not invoke directly. 81 | Builds all the zonotopes needed to solve ZPC. 82 | """ 83 | X0, W, V, Av, U, Y = zonotopes.X0, zonotopes.W, zonotopes.V, zonotopes.Av, zonotopes.U, zonotopes.Y 84 | assert X0.dimension == W.dimension and X0.dimension == self.dim_y \ 85 | and V.dimension == W.dimension and Av.dimension == V.dimension and Y.dimension == X0.dimension, \ 86 | 'The zonotopes do not have the correct dimension' 87 | 88 | self.zonotopes = zonotopes 89 | Mw = concatenate_zonotope(W, self.num_samples - 1) 90 | Mv = concatenate_zonotope(V, self.num_samples - 1) 91 | Mav = concatenate_zonotope(Av, self.num_samples - 1) 92 | 93 | self.Msigma = compute_IO_LTI_matrix_zonotope(self.dataset.Ym, self.dataset.Yp, self.dataset.Um, Mw, Mv, Mav).reduce(1) 94 | 95 | def build_problem(self, 96 | zonotopes: SystemZonotopes, 97 | horizon: int, 98 | build_loss: Callable[[cp.Variable, cp.Variable], Expression], 99 | build_constraints: Optional[Callable[[cp.Variable, cp.Variable], Optional[List[Constraint]]]] = None, 100 | Msigma_regularizer: float = 0.1) -> OptimizationProblem: 101 | """ 102 | Builds the ZPC optimization problem 103 | For more info check section 3.2 in https://arxiv.org/pdf/2103.14110.pdf 104 | 105 | :param zonotopes: System zonotopes 106 | :param horizon: Horizon length 107 | :param build_loss: Callback function that takes as input an (input,output) tuple of data 108 | of shape (TxM), where T is the horizon length and M is the feature size 109 | The callback should return a scalar value of type Expression 110 | :param build_constraints: Callback function that takes as input an (input,output) tuple of data 111 | of shape (TxM), where T is the horizon length and M is the feature size 112 | The callback should return a list of constraints. 113 | :return: Parameters of the optimization problem 114 | """ 115 | assert build_loss is not None, "Loss function callback cannot be none" 116 | 117 | self.optimization_problem = None 118 | self._build_zonotopes(zonotopes) 119 | 120 | print(f"Order of M {self.Msigma.order}") 121 | 122 | # Build variables 123 | y0 = cp.Parameter(shape=(self.dim_y)) 124 | u = cp.Variable(shape=(horizon, self.dim_u)) 125 | y = cp.Variable(shape=(horizon, self.dim_y)) 126 | su = cp.Variable(shape=(horizon, self.dim_y), nonneg=True) 127 | sl = cp.Variable(shape=(horizon, self.dim_y), nonneg=True) 128 | 129 | R = [CVXZonotope(y0, np.zeros((self.dim_y, 1)))] 130 | U = [CVXZonotope(u[i, :], np.zeros((self.dim_u, 1))) for i in range(horizon)] 131 | Z = self.zonotopes.W + self.zonotopes.V + (-1 *self.zonotopes.Av) 132 | 133 | leftY = self.zonotopes.Y.interval.left_limit.flatten() 134 | rightY = self.zonotopes.Y.interval.right_limit.flatten() 135 | 136 | constraints = [ 137 | y >= np.array([leftY.flatten()] * horizon), 138 | y <= np.array([rightY.flatten()] * horizon), 139 | u <= self.zonotopes.U.interval.right_limit, 140 | u >= self.zonotopes.U.interval.left_limit 141 | ] 142 | 143 | betas = [] 144 | 145 | Msigma = self.Msigma.reduce(max(1, int(self.Msigma.order * Msigma_regularizer))) 146 | print(f'Regularized MSigma: old order {self.Msigma.order} - new order {Msigma.order}') 147 | 148 | for i in range(horizon): 149 | print(f'Building for step {i}') 150 | XU = R[i].cartesian_product(U[i]) 151 | Rnew: CVXZonotope = (self.Msigma * XU) + Z 152 | 153 | 154 | R.append(Rnew) 155 | 156 | leftR = Rnew.interval.left_limit 157 | rightR = Rnew.interval.right_limit 158 | 159 | new_beta = cp.Variable(shape=(Rnew.num_generators)) 160 | betas.append(new_beta) 161 | constraints.extend([ 162 | y[i] == Rnew.center,# + su[i], 163 | rightR + su[i] <= rightY, 164 | leftR -sl[i] >= leftY 165 | ]) 166 | 167 | 168 | _constraints = build_constraints(u, y) if build_constraints is not None else (None, None) 169 | 170 | for idx, constraint in enumerate(_constraints): 171 | if constraint is None or not isinstance(constraint, Constraint) or not constraint.is_dcp(): 172 | raise Exception(f'Constraint {idx} is not defined or is not convex.') 173 | 174 | constraints.extend([] if _constraints is None else _constraints) 175 | 176 | # Build loss 177 | _loss = build_loss(u, y) 178 | 179 | if _loss is None or not isinstance(_loss, Expression) or not _loss.is_dcp(): 180 | raise Exception('Loss function is not defined or is not convex!') 181 | 182 | _regularizers = 0#1e1*(cp.norm(su, p=1) )#+ cp.norm(sl, p=1)) 183 | problem_loss = _loss + _regularizers 184 | 185 | # Solve problem 186 | objective = cp.Minimize(problem_loss) 187 | 188 | try: 189 | problem = cp.Problem(objective, constraints) 190 | except cp.SolverError as e: 191 | raise Exception(f'Error while constructing the DeePC problem. Details: {e}') 192 | 193 | self.optimization_problem = OptimizationProblem( 194 | variables = OptimizationProblemVariables(y0=y0, u=u, y=y, su=su, sl=sl, beta_u=None), 195 | constraints = constraints, 196 | objective_function = problem_loss, 197 | problem = problem, 198 | reachable_set = R 199 | ) 200 | 201 | return self.optimization_problem 202 | 203 | def solve( 204 | self, 205 | y0: np.ndarray, 206 | **cvxpy_kwargs 207 | ) -> Tuple[np.ndarray, Dict[str, Union[float, np.ndarray, OptimizationProblemVariables]]]: 208 | """ 209 | Solves the DeePC optimization problem 210 | For more info check alg. 2 in https://arxiv.org/pdf/1811.05890.pdf 211 | 212 | :param y0: The initial output 213 | :param cvxpy_kwargs: All arguments that need to be passed to the cvxpy solve method. 214 | :return u_optimal: Optimal input signal to be applied to the system, of length `horizon` 215 | :return info: A dictionary with 5 keys: 216 | info['variables']: variables of the optimization problem 217 | info['value']: value of the optimization problem 218 | info['u_optimal']: the same as the first value returned by this function 219 | """ 220 | assert len(y0) == self.dim_y, f"Invalid size" 221 | assert self.optimization_problem is not None, "Problem was not built" 222 | 223 | 224 | self.optimization_problem.variables.y0.value = y0 225 | try: 226 | #import pdb 227 | #pdb.set_trace() 228 | result = self.optimization_problem.problem.solve(**cvxpy_kwargs) 229 | except cp.SolverError as e: 230 | with open('zpc_logs.txt', 'w') as f: 231 | print(f'Error while solving the DeePC problem. Details: {e}', file=f) 232 | raise Exception(f'Error while solving the DeePC problem. Details: {e}') 233 | 234 | if np.isinf(result): 235 | raise Exception('Problem is unbounded') 236 | 237 | u_optimal = self.optimization_problem.variables.u.value 238 | info = { 239 | 'value': result, 240 | 'variables': self.optimization_problem.variables, 241 | 'u_optimal': u_optimal, 242 | 'reachable_set': self.optimization_problem.reachable_set 243 | } 244 | 245 | return u_optimal, info -------------------------------------------------------------------------------- /pyzpc/zpc_simplified.py: -------------------------------------------------------------------------------- 1 | from cmath import isinf 2 | from re import S 3 | import numpy as np 4 | import cvxpy as cp 5 | from typing import Tuple, Callable, List, Optional, Union, Dict 6 | from cvxpy.expressions.expression import Expression 7 | from cvxpy.constraints.constraint import Constraint 8 | from pyzonotope import ( 9 | concatenate_zonotope, 10 | MatrixZonotope, 11 | Zonotope, 12 | CVXZonotope) 13 | 14 | from pydatadrivenreachability import compute_IO_LTI_matrix_zonotope 15 | 16 | from pyzpc.utils import ( 17 | Data, 18 | DataDrivenDataset, 19 | SystemZonotopes, 20 | OptimizationProblem, 21 | OptimizationProblemVariables) 22 | import sys 23 | #sys.setrecursionlimit(10000) 24 | 25 | 26 | class ZPC(object): 27 | optimization_problem: Union[OptimizationProblem,None] = None 28 | dataset: DataDrivenDataset 29 | zonotopes: SystemZonotopes 30 | Msigma: MatrixZonotope 31 | 32 | def __init__(self, data: Data): 33 | """ 34 | Solves the ZPC optimization problem 35 | See also https://arxiv.org/pdf/2103.14110.pdf 36 | 37 | :param data: A tuple of input/output data. Data should have shape TxM 38 | where T is the batch size and M is the number of features 39 | """ 40 | self.update_identification_data(data) 41 | 42 | @property 43 | def num_samples(self) -> int: 44 | """ Return the number of samples used to estimate the Matrix Zonotope Msigma """ 45 | return self.dataset.Um.shape[0] + 1 46 | 47 | @property 48 | def dim_u(self) -> int: 49 | """ Return the dimensionality of u (the control signal) """ 50 | return self.dataset.Um.shape[1] 51 | 52 | @property 53 | def dim_y(self) -> int: 54 | """ Return the dimensionality of y (the output signal) """ 55 | return self.dataset.Yp.shape[1] 56 | 57 | def update_identification_data(self, data: Data): 58 | """ 59 | Update identification data matrices of ZPC. You need to rebuild the optimization problem 60 | after calling this funciton. 61 | 62 | :param data: A tuple of input/output data. Data should have shape TxM 63 | where T is the batch size and M is the number of features 64 | """ 65 | assert len(data.u.shape) == 2, \ 66 | "Data needs to be shaped as a TxM matrix (T is the number of samples and M is the number of features)" 67 | assert len(data.y.shape) == 2, \ 68 | "Data needs to be shaped as a TxM matrix (T is the number of samples and M is the number of features)" 69 | assert data.y.shape[0] == data.u.shape[0], \ 70 | "Input/output data must have the same length" 71 | 72 | Ym = data.y[:-1] 73 | Yp = data.y[1:] 74 | Um = data.u[:-1] 75 | self.dataset = DataDrivenDataset(Yp, Ym, Um) 76 | self.optimization_problem = None 77 | 78 | def _build_zonotopes(self, zonotopes: SystemZonotopes): 79 | """ 80 | [Private method] Do not invoke directly. 81 | Builds all the zonotopes needed to solve ZPC. 82 | """ 83 | X0, W, V, Av, U, Y = zonotopes.X0, zonotopes.W, zonotopes.V, zonotopes.Av, zonotopes.U, zonotopes.Y 84 | assert X0.dimension == W.dimension and X0.dimension == self.dim_y \ 85 | and V.dimension == W.dimension and Av.dimension == V.dimension and Y.dimension == X0.dimension, \ 86 | 'The zonotopes do not have the correct dimension' 87 | 88 | self.zonotopes = zonotopes 89 | Mw = concatenate_zonotope(W, self.num_samples - 1) 90 | Mv = concatenate_zonotope(V, self.num_samples - 1) 91 | Mav = concatenate_zonotope(Av, self.num_samples - 1) 92 | 93 | self.Msigma = compute_IO_LTI_matrix_zonotope(self.dataset.Ym, self.dataset.Yp, self.dataset.Um, Mw, Mv, Mav).reduce(1) 94 | 95 | def build_problem(self, 96 | zonotopes: SystemZonotopes, 97 | horizon: int, 98 | build_loss: Callable[[cp.Variable, cp.Variable], Expression], 99 | build_constraints: Optional[Callable[[cp.Variable, cp.Variable], Optional[List[Constraint]]]] = None 100 | ) -> OptimizationProblem: 101 | """ 102 | Builds the ZPC optimization problem 103 | For more info check section 3.2 in https://arxiv.org/pdf/2103.14110.pdf 104 | 105 | :param zonotopes: System zonotopes 106 | :param horizon: Horizon length 107 | :param build_loss: Callback function that takes as input an (input,output) tuple of data 108 | of shape (TxM), where T is the horizon length and M is the feature size 109 | The callback should return a scalar value of type Expression 110 | :param build_constraints: Callback function that takes as input an (input,output) tuple of data 111 | of shape (TxM), where T is the horizon length and M is the feature size 112 | The callback should return a list of constraints. 113 | :return: Parameters of the optimization problem 114 | """ 115 | assert build_loss is not None, "Loss function callback cannot be none" 116 | 117 | self.optimization_problem = None 118 | self._build_zonotopes(zonotopes) 119 | 120 | print(f"Order of M {self.Msigma.order}") 121 | 122 | # Build variables 123 | y0 = cp.Parameter(shape=(self.dim_y)) 124 | u = cp.Variable(shape=(horizon, self.dim_u)) 125 | y = cp.Variable(shape=(horizon, self.dim_y)) 126 | su = cp.Variable(shape=(horizon, self.dim_y), nonneg=True) 127 | sl = cp.Variable(shape=(horizon, self.dim_y), nonneg=True) 128 | beta_u = cp.Variable(shape=(horizon, self.zonotopes.U.num_generators)) 129 | 130 | 131 | R = [CVXZonotope(y0, np.zeros((self.dim_y, 1)))] 132 | U = [CVXZonotope(u[i, :], np.zeros((self.dim_u, 1))) for i in range(horizon)] 133 | Z = self.zonotopes.W + self.zonotopes.V + (-1 *self.zonotopes.Av) 134 | 135 | leftY = self.zonotopes.Y.interval.left_limit.flatten() 136 | rightY = self.zonotopes.Y.interval.right_limit.flatten() 137 | 138 | constraints = [ 139 | y >= np.array([leftY.flatten()] * horizon), 140 | y <= np.array([rightY.flatten()] * horizon), 141 | beta_u >= -1., 142 | beta_u <= 1., 143 | u == np.array([self.zonotopes.U.center] * horizon) + (beta_u @ self.zonotopes.U.generators.T), 144 | ] 145 | 146 | for i in range(horizon): 147 | print(f'Building for step {i}') 148 | XU = R[i].cartesian_product(U[i]) 149 | Rnew: CVXZonotope = (XU * self.Msigma.center) + Z 150 | 151 | R.append(Rnew) 152 | 153 | leftR = Rnew.interval.left_limit 154 | rightR = Rnew.interval.right_limit 155 | 156 | constraints.extend([ 157 | y[i] == Rnew.center, 158 | rightR + su[i] <= rightY, 159 | leftR -sl[i] >= leftY 160 | ]) 161 | 162 | _constraints = build_constraints(u, y) if build_constraints is not None else (None, None) 163 | 164 | for idx, constraint in enumerate(_constraints): 165 | if constraint is None or not isinstance(constraint, Constraint) or not constraint.is_dcp(): 166 | raise Exception(f'Constraint {idx} is not defined or is not convex.') 167 | 168 | constraints.extend([] if _constraints is None else _constraints) 169 | 170 | # Build loss 171 | _loss = build_loss(u, y) 172 | 173 | if _loss is None or not isinstance(_loss, Expression) or not _loss.is_dcp(): 174 | raise Exception('Loss function is not defined or is not convex!') 175 | 176 | problem_loss = _loss 177 | 178 | # Solve problem 179 | objective = cp.Minimize(problem_loss) 180 | 181 | try: 182 | problem = cp.Problem(objective, constraints) 183 | except cp.SolverError as e: 184 | raise Exception(f'Error while constructing the DeePC problem. Details: {e}') 185 | 186 | self.optimization_problem = OptimizationProblem( 187 | variables = OptimizationProblemVariables(y0=y0, u=u, y=y, s=s, beta_u=beta_u), 188 | constraints = constraints, 189 | objective_function = problem_loss, 190 | problem = problem 191 | ) 192 | 193 | return self.optimization_problem 194 | 195 | def solve( 196 | self, 197 | y0: np.ndarray, 198 | **cvxpy_kwargs 199 | ) -> Tuple[np.ndarray, Dict[str, Union[float, np.ndarray, OptimizationProblemVariables]]]: 200 | """ 201 | Solves the DeePC optimization problem 202 | For more info check alg. 2 in https://arxiv.org/pdf/1811.05890.pdf 203 | 204 | :param y0: The initial output 205 | :param cvxpy_kwargs: All arguments that need to be passed to the cvxpy solve method. 206 | :return u_optimal: Optimal input signal to be applied to the system, of length `horizon` 207 | :return info: A dictionary with 5 keys: 208 | info['variables']: variables of the optimization problem 209 | info['value']: value of the optimization problem 210 | info['u_optimal']: the same as the first value returned by this function 211 | """ 212 | assert len(y0) == self.dim_y, f"Invalid size" 213 | assert self.optimization_problem is not None, "Problem was not built" 214 | 215 | 216 | self.optimization_problem.variables.y0.value = y0 217 | try: 218 | #import pdb 219 | #pdb.set_trace() 220 | result = self.optimization_problem.problem.solve(**cvxpy_kwargs) 221 | except cp.SolverError as e: 222 | with open('zpc_logs.txt', 'w') as f: 223 | print(f'Error while solving the DeePC problem. Details: {e}', file=f) 224 | raise Exception(f'Error while solving the DeePC problem. Details: {e}') 225 | 226 | if np.isinf(result): 227 | raise Exception('Problem is unbounded') 228 | 229 | u_optimal = self.optimization_problem.variables.u.value 230 | info = { 231 | 'value': result, 232 | 'variables': self.optimization_problem.variables, 233 | 'u_optimal': u_optimal 234 | } 235 | 236 | return u_optimal, info -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | from os import path 3 | 4 | 5 | setup(name = 'pyzpc', 6 | packages=find_packages(), 7 | version = '0.0.1', 8 | description = 'Python library that implements ZPC: Zonotopic Data-Driven Predictive Control.', 9 | url = 'https://github.com/rssalessio/PyZPC', 10 | author = 'Alessio Russo', 11 | author_email = 'alessior@kth.se', 12 | install_requires=['numpy', 'scipy', 'cvxpy'], 13 | license='MIT', 14 | zip_safe=False, 15 | python_requires='>=3.7', 16 | ) --------------------------------------------------------------------------------