├── .gitignore ├── .travis.yml ├── README.md ├── examples ├── __init__.py ├── cart_pole_dynamics.m ├── disjunctive_inequalities.ipynb ├── hybrid_mpc.ipynb ├── hybrid_mpc_2.ipynb ├── linear_mpc.ipynb ├── maximal_constraint_admissible_set.ipynb └── pwa_from_urdf │ ├── __init__.py │ ├── cart_pole.urdf │ ├── cart_pole_from_urdf.ipynb │ └── utils.py ├── pympc ├── __init__.py ├── control │ ├── __init__.py │ └── controllers.py ├── dynamics │ ├── __init__.py │ ├── discrete_time_systems.py │ ├── discretization_methods.py │ └── utils.py ├── geometry │ ├── __init__.py │ ├── polyhedron.py │ └── utils.py ├── optimization │ ├── __init__.py │ ├── parametric_programs.py │ ├── programs.py │ └── solvers │ │ ├── __init__.py │ │ ├── drake.py │ │ ├── gurobi.py │ │ └── pnnls.py ├── plot.py └── test │ ├── __init__.py │ ├── test_control │ ├── __init__.py │ └── test_controllers.py │ ├── test_dynamics │ ├── __init__.py │ ├── test_discrete_time_systems.py │ ├── test_discretization_methods.py │ └── test_utils.py │ ├── test_geometry │ ├── __init__.py │ ├── test_polyhedron.py │ └── test_utils.py │ └── test_optimization │ ├── __init__.py │ ├── test_parametric_programs.py │ ├── test_programs.py │ └── test_solvers.py ├── requirements.txt └── setup.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *.npy 3 | *.dot 4 | *.pdf 5 | gurobi.log 6 | .DS_Store 7 | .ipynb_checkpoints 8 | venv 9 | build 10 | dist 11 | *.egg-info 12 | .pytest_cache 13 | venv* 14 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | dist: trusty 3 | python: 4 | - "2.7" 5 | notifications: 6 | email: false 7 | install: 8 | - pip install -r requirements.txt 9 | - pip install nose 10 | - pip install codecov 11 | - touch gurobipy.py 12 | script: "python -m nose pympc --with-coverage --exclude=test_feedforward_feedback_and_get_mpqp --exclude=test_linear_program_gurobi --exclude=test_quadratic_program_gurobi --exclude=test_mixed_integer_quadratic_program_gurobi --exclude=test_linear_programming_solver_gurobi --exclude=test_quadratic_programming_solver_gurobi --exclude=test_mixed_integer_quadratic_programming_solver_gurobi" 13 | after_success: 14 | - codecov -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pympc 2 | 3 | pympc is a pyhton toolbox for Model Predictive Control (MPC) of linear and hybrid systems. 4 | 5 | This toolbox implements several algorithms for: 6 | - polyhedral computations, 7 | - computation of maximum constraint-admissible invariant sets, 8 | - MPC of linear systems, 9 | - explicit MPC of linear systems, 10 | - MPC for hybrid systems in piecewise-affine form. 11 | 12 | # Examples 13 | 14 | The `examples` folder contains some introductory Jupyter notebooks. 15 | -------------------------------------------------------------------------------- /examples/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/examples/__init__.py -------------------------------------------------------------------------------- /examples/cart_pole_dynamics.m: -------------------------------------------------------------------------------- 1 | clear 2 | clc 3 | 4 | % cart-pole hybrid dynamics 5 | % mode 0 -> if: pp(1) < -d, for all u 6 | % mode 1 -> if: -d <= pp(1) <= d, for all u 7 | % mode 2 -> if: pp(1) > d, for all u 8 | 9 | % symbolic variables 10 | syms q1 q2 qd1 qd2 u mc mp l d k g 11 | assume([q1, q2, qd1, qd2, u, mc, mp, l, d, k, g], 'real') 12 | 13 | % numeric parameter 14 | mc_n = 1; 15 | mp_n = 1; 16 | l_n = 1; 17 | d_n = 1; 18 | k_n = 100; 19 | g_n = 10; 20 | par = [mc, mp, l, d, k, g]; 21 | par_n = [mc_n, mp_n, l_n, d_n, k_n, g_n]; 22 | 23 | % equilibrium point 24 | x_eq = [0; 0; 0; 0]; 25 | u_eq = 0; 26 | 27 | % gravity 28 | gr = [0; -g]; 29 | 30 | % state 31 | q = [q1; q2]; 32 | qd = [qd1; qd2]; 33 | x = [q; qd]; 34 | 35 | % positions 36 | pc = [q1; 0]; 37 | pp = [q1-l*sin(q2); l*cos(q2)]; 38 | 39 | % velocities 40 | vc = jacobian(pc, q)*qd; 41 | vp = jacobian(pp, q)*qd; 42 | 43 | % lagrangian functions 44 | T = .5*mc*(vc'*vc) + .5*mp*(vp'*vp); 45 | U0 = - mp*(gr'*pp) + .5*k*(pp(1) + d)^2; 46 | U1 = - mp*(gr'*pp); 47 | U2 = - mp*(gr'*pp) + .5*k*(pp(1) - d)^2; 48 | L0 = T - U0; 49 | L1 = T - U1; 50 | L2 = T - U2; 51 | 52 | % generalized forces 53 | f = [u; 0]; 54 | Q = (f'*jacobian(pc, q))'; 55 | 56 | % equations of motion 57 | M0 = jacobian(jacobian(L0, qd)', qd); 58 | M1 = jacobian(jacobian(L1, qd)', qd); 59 | M2 = jacobian(jacobian(L2, qd)', qd); 60 | C0 = jacobian(jacobian(L0, qd)', q)*qd - jacobian(L0, q)'; 61 | C1 = jacobian(jacobian(L1, qd)', q)*qd - jacobian(L1, q)'; 62 | C2 = jacobian(jacobian(L2, qd)', q)*qd - jacobian(L2, q)'; 63 | 64 | % state space form 65 | f1 = simplify([qd; M1\(Q-C1)]); 66 | f2 = simplify([qd; M2\(Q-C2)]); 67 | f0 = simplify([qd; M0\(Q-C0)]); 68 | 69 | % symbolic linearization 70 | var = [x', u']; 71 | var_n = [x_eq', u_eq']; 72 | A0 = subs(jacobian(f0, x), var, var_n); 73 | A1 = subs(jacobian(f1, x), var, var_n); 74 | A2 = subs(jacobian(f2, x), var, var_n); 75 | B0 = subs(jacobian(f0, u), var, var_n); 76 | B1 = subs(jacobian(f1, u), var, var_n); 77 | B2 = subs(jacobian(f2, u), var, var_n); 78 | c0 = subs(f0, var, var_n); 79 | c1 = subs(f1, var, var_n); 80 | c2 = subs(f2, var, var_n); 81 | 82 | % numeric linearization 83 | var = [par, x', u']; 84 | var_n = [par_n, x_eq', u_eq']; 85 | A0_n = subs(jacobian(f0, x), var, var_n); 86 | A1_n = subs(jacobian(f1, x), var, var_n); 87 | A2_n = subs(jacobian(f2, x), var, var_n); 88 | B0_n = subs(jacobian(f0, u), var, var_n); 89 | B1_n = subs(jacobian(f1, u), var, var_n); 90 | B2_n = subs(jacobian(f2, u), var, var_n); 91 | c0_n = subs(f0, var, var_n); 92 | c1_n = subs(f1, var, var_n); 93 | c2_n = subs(f2, var, var_n); 94 | -------------------------------------------------------------------------------- /examples/pwa_from_urdf/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/examples/pwa_from_urdf/__init__.py -------------------------------------------------------------------------------- /examples/pwa_from_urdf/cart_pole.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 1 111 | 112 | 113 | -------------------------------------------------------------------------------- /examples/pwa_from_urdf/cart_pole_from_urdf.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Piecewise Affine Model from URDF Description of the Robot" 8 | ] 9 | }, 10 | { 11 | "cell_type": "code", 12 | "execution_count": null, 13 | "metadata": { 14 | "collapsed": true 15 | }, 16 | "outputs": [], 17 | "source": [ 18 | "%load_ext autoreload\n", 19 | "%autoreload 2" 20 | ] 21 | }, 22 | { 23 | "cell_type": "code", 24 | "execution_count": null, 25 | "metadata": { 26 | "collapsed": true 27 | }, 28 | "outputs": [], 29 | "source": [ 30 | "import numpy as np\n", 31 | "import matplotlib.pyplot as plt\n", 32 | "from IPython.display import HTML" 33 | ] 34 | }, 35 | { 36 | "cell_type": "code", 37 | "execution_count": null, 38 | "metadata": { 39 | "collapsed": true 40 | }, 41 | "outputs": [], 42 | "source": [ 43 | "from underactuated import PlanarRigidBodyVisualizer\n", 44 | "from pydrake.all import (FloatingBaseType,\n", 45 | " RigidBodyTree,\n", 46 | " RigidBodyPlant,\n", 47 | " DiagramBuilder,\n", 48 | " ZeroOrderHold,\n", 49 | " SignalLogger,\n", 50 | " Simulator)" 51 | ] 52 | }, 53 | { 54 | "cell_type": "code", 55 | "execution_count": null, 56 | "metadata": { 57 | "collapsed": true 58 | }, 59 | "outputs": [], 60 | "source": [ 61 | "from pympc.geometry.polyhedron import Polyhedron\n", 62 | "from utils import pwa_from_RigidBodyPlant, Controller" 63 | ] 64 | }, 65 | { 66 | "cell_type": "code", 67 | "execution_count": null, 68 | "metadata": { 69 | "collapsed": true 70 | }, 71 | "outputs": [], 72 | "source": [ 73 | "# linearization points for the angle of the pole\n", 74 | "theta_min = - np.pi * .3\n", 75 | "theta_max = np.pi * .3\n", 76 | "n_linearizations = 3\n", 77 | "\n", 78 | "# linearization points for the entire state x = (q_cart, q_pole, v_cart, v_pole)\n", 79 | "linearization_points = [np.zeros(4) for i in range(n_linearizations)]\n", 80 | "for i, theta in enumerate(np.linspace(theta_min, theta_max, n_linearizations)):\n", 81 | " linearization_points[i][1] = theta" 82 | ] 83 | }, 84 | { 85 | "cell_type": "code", 86 | "execution_count": null, 87 | "metadata": { 88 | "collapsed": true 89 | }, 90 | "outputs": [], 91 | "source": [ 92 | "# boundaries for the state (despite of the mode the state has to lie in X)\n", 93 | "theta_step = (theta_max - theta_min) / (n_linearizations - 1)\n", 94 | "x_min = np.array([\n", 95 | " -3.,\n", 96 | " theta_min - theta_step,\n", 97 | " -10.,\n", 98 | " -10.\n", 99 | "])\n", 100 | "x_max = np.array([\n", 101 | " 3.,\n", 102 | " theta_max + theta_step,\n", 103 | " 10.,\n", 104 | " 10.\n", 105 | "])\n", 106 | "X = Polyhedron.from_bounds(x_min, x_max)" 107 | ] 108 | }, 109 | { 110 | "cell_type": "code", 111 | "execution_count": null, 112 | "metadata": { 113 | "collapsed": true 114 | }, 115 | "outputs": [], 116 | "source": [ 117 | "# boundaries for the input --force on the cart-- (despite of the mode the inpu has to lie in U)\n", 118 | "u_min = np.array([-100.])\n", 119 | "u_max = np.array([100.])\n", 120 | "U = Polyhedron.from_bounds(u_min, u_max)" 121 | ] 122 | }, 123 | { 124 | "cell_type": "code", 125 | "execution_count": null, 126 | "metadata": { 127 | "collapsed": true 128 | }, 129 | "outputs": [], 130 | "source": [ 131 | "# parse RigidBodyPlant from urdf\n", 132 | "tree = RigidBodyTree(\"cart_pole.urdf\", FloatingBaseType.kFixed)\n", 133 | "plant = RigidBodyPlant(tree)" 134 | ] 135 | }, 136 | { 137 | "cell_type": "code", 138 | "execution_count": null, 139 | "metadata": { 140 | "collapsed": true 141 | }, 142 | "outputs": [], 143 | "source": [ 144 | "# sampling time\n", 145 | "h = .05\n", 146 | "\n", 147 | "# get piecewise affine system\n", 148 | "pwa = pwa_from_RigidBodyPlant(plant, linearization_points, X, U, h)" 149 | ] 150 | }, 151 | { 152 | "cell_type": "code", 153 | "execution_count": null, 154 | "metadata": { 155 | "collapsed": true 156 | }, 157 | "outputs": [], 158 | "source": [ 159 | "# parameters of the quadratic controller\n", 160 | "N = 20\n", 161 | "Q = np.eye(pwa.nx)/100.\n", 162 | "R = np.eye(pwa.nu)/100.\n", 163 | "P = Q\n", 164 | "X_N = Polyhedron.from_bounds(*[np.zeros(pwa.nx)]*2)\n", 165 | "\n", 166 | "# drake controller (System)\n", 167 | "controller = Controller(pwa, N, Q, R, Q, X_N)" 168 | ] 169 | }, 170 | { 171 | "cell_type": "code", 172 | "execution_count": null, 173 | "metadata": { 174 | "collapsed": true 175 | }, 176 | "outputs": [], 177 | "source": [ 178 | "# blocks of the diagram\n", 179 | "builder = DiagramBuilder()\n", 180 | "robot = builder.AddSystem(plant)\n", 181 | "controller = builder.AddSystem(controller)\n", 182 | "zoh = builder.AddSystem(ZeroOrderHold(.1, 1))" 183 | ] 184 | }, 185 | { 186 | "cell_type": "code", 187 | "execution_count": null, 188 | "metadata": { 189 | "collapsed": true 190 | }, 191 | "outputs": [], 192 | "source": [ 193 | "# logger\n", 194 | "logger_freq = 30.\n", 195 | "logger = SignalLogger(plant.get_num_states())\n", 196 | "state_log = builder.AddSystem(logger)\n", 197 | "state_log._DeclarePeriodicPublish(1./logger_freq, 0.0)" 198 | ] 199 | }, 200 | { 201 | "cell_type": "code", 202 | "execution_count": null, 203 | "metadata": { 204 | "collapsed": true 205 | }, 206 | "outputs": [], 207 | "source": [ 208 | "# connect and build the diagram\n", 209 | "builder.Connect(robot.get_output_port(0), controller.get_input_port(0))\n", 210 | "builder.Connect(controller.get_output_port(0), zoh.get_input_port(0))\n", 211 | "builder.Connect(zoh.get_output_port(0), robot.get_input_port(0))\n", 212 | "builder.Connect(plant.get_output_port(0), state_log.get_input_port(0))\n", 213 | "diagram = builder.Build()" 214 | ] 215 | }, 216 | { 217 | "cell_type": "code", 218 | "execution_count": null, 219 | "metadata": { 220 | "collapsed": true 221 | }, 222 | "outputs": [], 223 | "source": [ 224 | "# set up sim\n", 225 | "simulator = Simulator(diagram)\n", 226 | "simulator.set_publish_every_time_step(False)\n", 227 | "integrator = simulator.get_mutable_integrator()\n", 228 | "integrator.set_fixed_step_mode(True)\n", 229 | "integrator.set_maximum_step_size(0.001)" 230 | ] 231 | }, 232 | { 233 | "cell_type": "code", 234 | "execution_count": null, 235 | "metadata": {}, 236 | "outputs": [], 237 | "source": [ 238 | "# simulate\n", 239 | "context = simulator.get_mutable_context()\n", 240 | "x0 = np.array([0., 0., 3., 0.])\n", 241 | "\n", 242 | "# since the ZOH block at t=0 returns its internal state, I have to compute the first feedback \"offline\"\n", 243 | "state_c = context.get_mutable_continuous_state_vector()\n", 244 | "state_c.SetFromVector(x0)\n", 245 | "state_d = context.get_mutable_discrete_state_vector()\n", 246 | "u0 = np.empty(pwa.nu)\n", 247 | "controller._DoCalcVectorOutput(context, x0, None, u0)\n", 248 | "state_d.SetFromVector(u0)\n", 249 | "\n", 250 | "# run sim\n", 251 | "simulator.StepTo(3.)" 252 | ] 253 | }, 254 | { 255 | "cell_type": "code", 256 | "execution_count": null, 257 | "metadata": {}, 258 | "outputs": [], 259 | "source": [ 260 | "# visualize\n", 261 | "viz = PlanarRigidBodyVisualizer(tree, xlim=[-1, 1], ylim=[-2.5, 3.])\n", 262 | "viz.fig.set_size_inches(10, 5)\n", 263 | "ani = viz.animate(state_log, 30, repeat=True)\n", 264 | "plt.close(viz.fig)\n", 265 | "HTML(ani.to_html5_video())" 266 | ] 267 | } 268 | ], 269 | "metadata": { 270 | "kernelspec": { 271 | "display_name": "Python 2", 272 | "language": "python", 273 | "name": "python2" 274 | }, 275 | "language_info": { 276 | "codemirror_mode": { 277 | "name": "ipython", 278 | "version": 2 279 | }, 280 | "file_extension": ".py", 281 | "mimetype": "text/x-python", 282 | "name": "python", 283 | "nbconvert_exporter": "python", 284 | "pygments_lexer": "ipython2", 285 | "version": "2.7.15" 286 | } 287 | }, 288 | "nbformat": 4, 289 | "nbformat_minor": 2 290 | } 291 | -------------------------------------------------------------------------------- /examples/pwa_from_urdf/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from copy import copy 3 | from scipy.spatial import Voronoi 4 | from copy import copy 5 | 6 | # drake 7 | from pydrake.all import (FirstOrderTaylorApproximation, 8 | BasicVector, 9 | VectorSystem, 10 | MathematicalProgram, 11 | SolutionResult) 12 | from pydrake.solvers.gurobi import GurobiSolver 13 | 14 | # pympc 15 | from pympc.geometry.polyhedron import Polyhedron 16 | from pympc.dynamics.discrete_time_systems import LinearSystem, AffineSystem, PieceWiseAffineSystem 17 | from pympc.optimization.programs import linear_program 18 | 19 | def _voronoi_nd(points): 20 | """ 21 | Given a list of n-dimensional points, returns the Voronoi partition of the space as a list of Polyhedron (pympc class). 22 | Uses the scipy wrapper of Voronoi from Qhull. 23 | Does not work in case of 1-dimensional points. 24 | 25 | Arguments 26 | ---------- 27 | points : list of numpy.ndarray 28 | Points for the Voronoi partition. 29 | 30 | Returns 31 | ---------- 32 | partition : list of Polyhedron 33 | Halfspace representation of all the cells of the partiotion. 34 | """ 35 | 36 | # loop over points 37 | partition = [] 38 | for i, point in enumerate(points): 39 | 40 | # h-rep of the cell for the point i 41 | A = [] 42 | b = [] 43 | 44 | # loop over the points that share a facet with point i 45 | for ridge in Voronoi(points).ridge_points: 46 | if i in ridge: 47 | 48 | # vector from point i to the neighbor 49 | bottom = point 50 | tip = points[ridge[1 - ridge.tolist().index(i)]] 51 | 52 | # hyperplane that separates point i and its neighbor 53 | Ai = tip - point 54 | center = bottom + (tip - bottom) / 2. 55 | bi = Ai.dot(center) 56 | A.append(Ai) 57 | b.append(bi) 58 | 59 | # assemble cell i and add to partition 60 | cell = Polyhedron(np.vstack(A), np.array(b)) 61 | cell.normalize() 62 | partition.append(cell) 63 | 64 | return partition 65 | 66 | def _voronoi_1d(points): 67 | """ 68 | Given a list of 1-dimensional points, returns the Voronoi partition of the space as a list of Polyhedron (pympc class). 69 | 70 | Arguments 71 | ---------- 72 | points : list of numpy.ndarray 73 | Points for the Voronoi partition. 74 | 75 | Returns 76 | ---------- 77 | partition : list of Polyhedron 78 | Halfspace representation of all the cells of the partiotion. 79 | """ 80 | 81 | # order point from smallest to biggest 82 | points = sorted(points) 83 | 84 | # loop from the smaller point 85 | polyhedra = [] 86 | for i, point in enumerate(points): 87 | 88 | # h-rep of the cell for the point i 89 | A = [] 90 | b = [] 91 | 92 | # get previous and next point (if any) 93 | tips = [] 94 | if i > 0: 95 | tips.append(points[i-1]) 96 | if i < len(points)-1: 97 | tips.append(points[i+1]) 98 | 99 | 100 | # vector from point i to the next/previous point 101 | for tip in tips: 102 | bottom = point 103 | center = bottom + (tip - bottom) / 2. 104 | 105 | # hyperplane that separates point i and its neighbor 106 | Ai = tip - point 107 | bi = Ai.dot(center) 108 | A.append(Ai) 109 | b.append(bi) 110 | 111 | # assemble cell i and add to partition 112 | polyhedron = Polyhedron(np.vstack(A), np.array(b)) 113 | polyhedron.normalize() 114 | polyhedra.append(polyhedron) 115 | 116 | return polyhedra 117 | 118 | def constrained_voronoi(points, X=None): 119 | """ 120 | Given a list of n-dimensional points, returns the Voronoi partition of the Polyhedron X as a list of Polyhedron. 121 | If X is None, returns the partition of the whole space. 122 | 123 | Arguments 124 | ---------- 125 | points : list of numpy.ndarray 126 | Points for the Voronoi partition. 127 | X : Polyhedron 128 | Set we want to partition. 129 | 130 | Returns 131 | ---------- 132 | partition : list of Polyhedron 133 | Halfspace representation of all the cells of the partiotion. 134 | """ 135 | 136 | # get indices of non-coincident coordinates 137 | nx = min(p.size for p in points) 138 | assert nx == max(p.size for p in points) 139 | indices = [i for i in range(nx) if not np.isclose(min([p[i] for p in points]), max([p[i] for p in points]))] 140 | 141 | # get voronoi partition without boundaries 142 | points_lower_dimensional = [p[indices] for p in points] 143 | if len(indices) == 1: 144 | vor = _voronoi_1d(points_lower_dimensional) 145 | else: 146 | vor = _voronoi_nd(points_lower_dimensional) 147 | 148 | # go back to the higher dimensional space 149 | partition = [Polyhedron(np.zeros((0,nx)), np.zeros(0)) for i in points] 150 | for i, cell in enumerate(vor): 151 | partition[i].add_inequality(cell.A, cell.b, indices=indices) 152 | 153 | # intersect with X is provided 154 | if X is not None: 155 | partition[i].add_inequality(X.A, X.b) 156 | 157 | return partition 158 | 159 | def pwa_from_RigidBodyPlant(plant, linearization_points, X, U, h, method='zero_order_hold'): 160 | """ 161 | 162 | Arguments 163 | ---------- 164 | plant : RigidBodyPlant 165 | RigidBodyPlant of the robot. 166 | linearization_points : list of numpy.ndarray 167 | Points in the state space where to linearize the dynamics. 168 | X : Polyhedron 169 | Overall bounds of the state space. 170 | U : Polyhedron 171 | Overall bounds of the control space. 172 | h : float 173 | Sampling time for the time discretization of the PWA system. 174 | method : string 175 | Method used to discretize each piece of the PWA dynamics ('explicit_euler' or 'zero_order_hold'). 176 | 177 | Returns 178 | ---------- 179 | PWA : PieceWiseAffineSystem 180 | """ 181 | 182 | # partition of the state space 183 | X_partition = constrained_voronoi(linearization_points, X) 184 | domains = [Xi.cartesian_product(U) for Xi in X_partition] 185 | 186 | # create context 187 | context = plant.CreateDefaultContext() 188 | state = context.get_mutable_continuous_state_vector() 189 | input = BasicVector(np.array([0.])) 190 | context.FixInputPort(0, input) 191 | 192 | # affine systems 193 | affine_systems = [] 194 | for x in linearization_points: 195 | state.SetFromVector(x) 196 | taylor_approx = FirstOrderTaylorApproximation(plant, context) 197 | affine_system = AffineSystem.from_continuous( 198 | taylor_approx.A(), 199 | taylor_approx.B(), 200 | taylor_approx.f0(), 201 | h, 202 | method 203 | ) 204 | affine_systems.append(affine_system) 205 | 206 | return PieceWiseAffineSystem(affine_systems, domains) 207 | 208 | class Controller(VectorSystem): 209 | """ 210 | Wrapper for the HybridModelPredictiveController class from pympc. 211 | """ 212 | 213 | def __init__(self, S, N, Q, R, P, X_N): 214 | """ 215 | Arguments 216 | ---------- 217 | S : PieceWiseAffineSystem 218 | PWA system to be controlled. 219 | N : int 220 | Horizon of the optimal control problem. 221 | Q : numpy.ndarray 222 | Quadratic cost for the state. 223 | R : numpy.ndarray 224 | Quadratic cost for the input. 225 | P : numpy.ndarray 226 | Quadratic cost for the terminal state. 227 | X_N : Polyhedron 228 | Terminal set. 229 | """ 230 | 231 | # bouild drake controller 232 | VectorSystem.__init__(self, S.nx, S.nu) 233 | self.controller = HybridModelPredictiveController(S, N, Q, R, Q, X_N) 234 | 235 | def _DoCalcVectorOutput(self, context, plant_state, unused, plant_input): 236 | print('Controller called at time ' + str(context.get_time()) + ' with state ' + str(plant_state) + ' \r'), 237 | plant_input[:] = self.controller.feedback(plant_state) 238 | 239 | class HybridModelPredictiveController(object): 240 | 241 | def __init__(self, S, N, Q, R, P, X_N): 242 | 243 | # store inputs 244 | self.S = S 245 | self.N = N 246 | self.Q = Q 247 | self.R = R 248 | self.P = P 249 | self.X_N = X_N 250 | 251 | # mpMIQP 252 | self.build_mpmiqp() 253 | 254 | def build_mpmiqp(self): 255 | 256 | # express the constrained dynamics as a list of polytopes in the (x,u,x+)-space 257 | P = graph_representation(self.S) 258 | m = big_m(P) 259 | 260 | # initialize program 261 | self.prog = MathematicalProgram() 262 | self.x = [] 263 | self.u = [] 264 | self.d = [] 265 | obj = 0. 266 | self.binaries_lower_bound = [] 267 | 268 | # initial conditions (set arbitrarily to zero in the building phase) 269 | self.x.append(self.prog.NewContinuousVariables(self.S.nx)) 270 | self.initial_condition = [] 271 | for k in range(self.S.nx): 272 | self.initial_condition.append(self.prog.AddLinearConstraint(self.x[0][k] == 0.).evaluator()) 273 | 274 | # loop over time 275 | for t in range(self.N): 276 | 277 | # create input, mode and next state variables 278 | self.u.append(self.prog.NewContinuousVariables(self.S.nu)) 279 | self.d.append(self.prog.NewBinaryVariables(self.S.nm)) 280 | self.x.append(self.prog.NewContinuousVariables(self.S.nx)) 281 | 282 | # enforce constrained dynamics (big-m methods) 283 | xux = np.concatenate((self.x[t], self.u[t], self.x[t+1])) 284 | for i in range(self.S.nm): 285 | mi_sum = np.sum([m[i][j] * self.d[t][j] for j in range(self.S.nm) if j != i], axis=0) 286 | for k in range(P[i].A.shape[0]): 287 | self.prog.AddLinearConstraint(P[i].A[k].dot(xux) <= P[i].b[k] + mi_sum[k]) 288 | 289 | # SOS1 on the binaries 290 | self.prog.AddLinearConstraint(sum(self.d[t]) == 1.) 291 | 292 | # stage cost to the objective 293 | obj += .5 * self.u[t].dot(self.R).dot(self.u[t]) 294 | obj += .5 * self.x[t].dot(self.Q).dot(self.x[t]) 295 | 296 | # terminal constraint 297 | for k in range(self.X_N.A.shape[0]): 298 | self.prog.AddLinearConstraint(self.X_N.A[k].dot(self.x[self.N]) <= self.X_N.b[k]) 299 | 300 | # terminal cost 301 | obj += .5 * self.x[self.N].dot(self.P).dot(self.x[self.N]) 302 | self.objective = self.prog.AddQuadraticCost(obj) 303 | 304 | # set solver 305 | self.solver = GurobiSolver() 306 | self.prog.SetSolverOption(self.solver.solver_type(), 'OutputFlag', 1) 307 | 308 | 309 | def set_initial_condition(self, x0): 310 | for k, c in enumerate(self.initial_condition): 311 | c.UpdateLowerBound(x0[k:k+1]) 312 | c.UpdateUpperBound(x0[k:k+1]) 313 | 314 | def feedforward(self, x0): 315 | 316 | # overwrite initial condition 317 | self.set_initial_condition(x0) 318 | 319 | # solve MIQP 320 | result = self.solver.Solve(self.prog) 321 | 322 | # check feasibility 323 | if result != SolutionResult.kSolutionFound: 324 | return None, None, None, None 325 | 326 | # get cost 327 | obj = self.prog.EvalBindingAtSolution(self.objective)[0] 328 | 329 | # store argmin in list of vectors 330 | u = [self.prog.GetSolution(ut) for ut in self.u] 331 | x = [self.prog.GetSolution(xt) for xt in self.x] 332 | d = [self.prog.GetSolution(dt) for dt in self.d] 333 | 334 | # retrieve mode sequence and check integer feasibility 335 | ms = [np.argmax(dt) for dt in d] 336 | 337 | return u, x, ms, obj 338 | 339 | 340 | def feedback(self, x0): 341 | 342 | # get feedforward and extract first input 343 | u_feedforward = self.feedforward(x0)[0] 344 | if u_feedforward is None: 345 | return None 346 | 347 | return u_feedforward[0] 348 | 349 | def graph_representation(S): 350 | ''' 351 | For the PWA system S 352 | x+ = Ai x + Bi u + ci if Fi x + Gi u <= hi, 353 | returns the graphs of the dynamics (list of Polyhedron) 354 | [ Fi Gi 0] [ x] [ hi] 355 | [ Ai Bi -I] [ u] <= [-ci] 356 | [-Ai -Bi I] [x+] [ ci] 357 | ''' 358 | P = [] 359 | for i in range(S.nm): 360 | Di = S.domains[i] 361 | Si = S.affine_systems[i] 362 | Ai = np.vstack(( 363 | np.hstack((Di.A, np.zeros((Di.A.shape[0], S.nx)))), 364 | np.hstack((Si.A, Si.B, -np.eye(S.nx))), 365 | np.hstack((-Si.A, -Si.B, np.eye(S.nx))), 366 | )) 367 | bi = np.concatenate((Di.b, -Si.c, Si.c)) 368 | P.append(Polyhedron(Ai, bi)) 369 | return P 370 | 371 | def big_m(P_list, tol=1.e-6): 372 | ''' 373 | For the list of Polyhedron P_list in the from Pi = {x | Ai x <= bi} returns a list of lists of numpy arrays, where m[i][j] := max_{x in Pj} Ai x - bi. 374 | 375 | ''' 376 | m = [] 377 | for i, Pi in enumerate(P_list): 378 | mi = [] 379 | for j, Pj in enumerate(P_list): 380 | mij = [] 381 | for k in range(Pi.A.shape[0]): 382 | sol = linear_program(-Pi.A[k], Pj.A, Pj.b) 383 | mijk = - sol['min'] - Pi.b[k] 384 | if np.abs(mijk) < tol: 385 | mijk = 0. 386 | mij.append(mijk) 387 | mi.append(np.array(mij)) 388 | m.append(mi) 389 | return m -------------------------------------------------------------------------------- /pympc/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/pympc/__init__.py -------------------------------------------------------------------------------- /pympc/control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/pympc/control/__init__.py -------------------------------------------------------------------------------- /pympc/control/controllers.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from scipy.linalg import block_diag 5 | 6 | # internal inputs 7 | from pympc.dynamics.discrete_time_systems import AffineSystem, PieceWiseAffineSystem 8 | from pympc.optimization.parametric_programs import MultiParametricQuadraticProgram, MultiParametricMixedIntegerQuadraticProgram 9 | from pympc.optimization.programs import linear_program 10 | 11 | class ModelPredictiveController(object): 12 | """ 13 | Model predictive controller for linear systems, it solves the optimal control problem 14 | V*(x(0)) := min_{x(.), u(.)} 1/2 sum_{t=0}^{N-1} x'(t) Q x(t) + u'(t) R u(t) + \frac{1}{2} x'(N) P x(N) 15 | s.t. x(t+1) = A x(t) + B u(t), t=0, ..., N-1, 16 | (x(t), u(t)) in D, t=0, ..., N-1, 17 | x(N) in X_N, 18 | in order to get the optimal control seuquence (u(0), ..., u(N-1)). 19 | """ 20 | 21 | def __init__(self, S, N, Q, R, P, D, X_N): 22 | """ 23 | Initilizes the controller. 24 | 25 | Arguments 26 | ---------- 27 | S : instance of LinerSystem 28 | Linear system to be controlled. 29 | N : int 30 | Horizon of the optimal control problem. 31 | Q : numpy.ndarray 32 | Quadratic cost for the state. 33 | R : numpy.ndarray 34 | Quadratic cost for the input. 35 | P : numpy.ndarray 36 | Quadratic cost for the terminal state. 37 | D : instance of Polyhedron 38 | Stage constraint for state and inputs. 39 | X_N : instance of Polyhedron 40 | Terminal set. 41 | """ 42 | 43 | # store inputs 44 | self.S = S 45 | self.N = N 46 | self.Q = Q 47 | self.R = R 48 | self.P = P 49 | self.D = D 50 | self.X_N = X_N 51 | 52 | # initilize explicit solution 53 | self.explicit_solution = None 54 | 55 | # condense mpqp 56 | self.mpqp = self._condense_program() 57 | 58 | def _condense_program(self): 59 | """ 60 | Generates and stores the optimal control problem in condensed form. 61 | 62 | Returns 63 | ---------- 64 | instance of MultiParametricQuadraticProgram 65 | Condensed mpQP. 66 | """ 67 | 68 | # create fake PWA system and use PWA condenser 69 | c = np.zeros(self.S.nx) 70 | S = AffineSystem(self.S.A, self.S.B, c) 71 | S = PieceWiseAffineSystem([S], [self.D]) 72 | mode_sequence = [0]*self.N 73 | 74 | return condense_optimal_control_problem(S, self.Q, self.R, self.P, self.X_N, mode_sequence) 75 | 76 | def feedforward(self, x): 77 | """ 78 | Given the state x of the system, returns the optimal sequence of N inputs and the related cost. 79 | 80 | Arguments 81 | ---------- 82 | x : numpy.ndarray 83 | State of the system. 84 | 85 | Returns 86 | ---------- 87 | u_feedforward : list of numpy.ndarray 88 | Optimal control signals for t = 0, ..., N-1. 89 | V : float 90 | Optimal value function for the given state. 91 | """ 92 | 93 | # solve and check feasibility 94 | sol = self.mpqp.solve(x) 95 | if sol['min'] is None: 96 | return None, None 97 | 98 | # from vector to list of vectors 99 | u_feedforward = [sol['argmin'][self.S.nu*i : self.S.nu*(i+1)] for i in range(self.N)] 100 | V = sol['min'] 101 | 102 | return u_feedforward, V 103 | 104 | def feedback(self, x): 105 | """ 106 | Returns the optimal feedback for the given state x. 107 | 108 | Arguments 109 | ---------- 110 | x : numpy.ndarray 111 | State of the system. 112 | 113 | Returns 114 | ---------- 115 | u_feedback : numpy.ndarray 116 | Optimal feedback. 117 | """ 118 | 119 | # get feedforward and extract first input 120 | u_feedforward = self.feedforward(x)[0] 121 | if u_feedforward is None: 122 | return None 123 | 124 | return u_feedforward[0] 125 | 126 | def store_explicit_solution(self, **kwargs): 127 | """ 128 | Solves the mpqp (condensed optimal control problem) explicitly. 129 | 130 | Returns 131 | ---------- 132 | instance of ExplicitSolution 133 | Explicit solution of the underlying mpqp problem. 134 | """ 135 | 136 | self.explicit_solution = self.mpqp.explicit_solve(**kwargs) 137 | 138 | def feedforward_explicit(self, x): 139 | """ 140 | Finds the critical region where the state x is and returns the optimal feedforward and the cost to go. 141 | 142 | Arguments 143 | ---------- 144 | x : numpy.ndarray 145 | State of the system. 146 | 147 | Returns 148 | ---------- 149 | u_feedforward : list of numpy.ndarray 150 | Optimal control signals for t = 0, ..., N-1. 151 | V : float 152 | Optimal value function for the given state. 153 | """ 154 | 155 | # check that the explicit solution has been found 156 | if self.explicit_solution is None: 157 | raise ValueError('explicit solution not stored.') 158 | 159 | # evaluate lookup table 160 | u = self.explicit_solution.u(x) 161 | if u is not None: 162 | u = [u[t*self.S.nu:(t+1)*self.S.nu] for t in range(self.N)] 163 | 164 | return u, self.explicit_solution.V(x) 165 | 166 | def feedback_explicit(self, x): 167 | """ 168 | Finds the critical region where the state x is and returns the optimal feedback for the given state x. 169 | 170 | Arguments 171 | ---------- 172 | x : numpy.ndarray 173 | State of the system. 174 | 175 | Returns 176 | ---------- 177 | u_feedback : numpy.ndarray 178 | Optimal feedback. 179 | """ 180 | 181 | # get feedforward and extract first input 182 | u_feedforward = self.feedforward_explicit(x)[0] 183 | if u_feedforward is None: 184 | return None 185 | 186 | return u_feedforward[0] 187 | 188 | def plot_state_space_partition(self, print_active_set=False, **kwargs): 189 | """ 190 | Finds the critical region where the state x is, and returns the PWA feedforward. 191 | 192 | Arguments 193 | ---------- 194 | print_active_set : bool 195 | If True it prints the active set of each critical region in its center. 196 | """ 197 | 198 | # check that the required plot is 2d and that the solution is available 199 | if self.S.nx != 2: 200 | raise ValueError('can plot only 2-dimensional partitions.') 201 | if self.explicit_solution is None: 202 | raise ValueError('explicit solution not stored.') 203 | 204 | # plot every critical region with random colors 205 | for cr in self.explicit_solution.critical_regions: 206 | cr.polyhedron.plot(facecolor=np.random.rand(3), **kwargs) 207 | 208 | # if required print active sets 209 | if print_active_set: 210 | plt.text(cr.polyhedron.center[0], cr.polyhedron.center[1], str(cr.active_set)) 211 | 212 | def plot_optimal_value_function(self, resolution=100, **kwargs): 213 | """ 214 | Plots the level sets of the optimal value function V*(x). 215 | 216 | Arguments 217 | ---------- 218 | resolution : float 219 | Size of the grid for the contour plot. 220 | """ 221 | 222 | # check dimension of the state 223 | if self.S.nx != 2: 224 | raise ValueError('can plot only 2-dimensional value functions.') 225 | if self.explicit_solution is None: 226 | raise ValueError('explicit solution not stored.') 227 | 228 | # get feasible set 229 | feasible_set = self.mpqp.get_feasible_set() 230 | 231 | # create box containing the feasible set 232 | x_max = max([v[0] for v in feasible_set.vertices]) 233 | x_min = min([v[0] for v in feasible_set.vertices]) 234 | y_max = max([v[1] for v in feasible_set.vertices]) 235 | y_min = min([v[1] for v in feasible_set.vertices]) 236 | 237 | # create grid 238 | x = np.linspace(x_min, x_max, resolution) 239 | y = np.linspace(y_min, y_max, resolution) 240 | X, Y = np.meshgrid(x, y) 241 | 242 | # evaluate grid 243 | zs = np.array([self.explicit_solution.V(np.array([x,y])) for x,y in zip(np.ravel(X), np.ravel(Y))]) 244 | Z = zs.reshape(X.shape) 245 | 246 | # plot 247 | feasible_set.plot(**kwargs) 248 | cp = plt.contour(X, Y, Z) 249 | plt.colorbar(cp) 250 | plt.title(r'$V^*(x)$') 251 | 252 | class HybridModelPredictiveController(object): 253 | 254 | def __init__(self, S, N, Q, R, P, X_N): 255 | """ 256 | Initilizes the controller. 257 | Arguments 258 | ---------- 259 | S : instance of PieceWiseAffineSystem 260 | PWA system to be controlled. 261 | N : int 262 | Horizon of the optimal control problem. 263 | Q : numpy.ndarray 264 | Quadratic cost for the state. 265 | R : numpy.ndarray 266 | Quadratic cost for the input. 267 | P : numpy.ndarray 268 | Quadratic cost for the terminal state. 269 | X_N : instance of Polyhedron 270 | Terminal set. 271 | """ 272 | 273 | # store inputs 274 | self.S = S 275 | self.N = N 276 | self.Q = Q 277 | self.R = R 278 | self.P = P 279 | self.X_N = X_N 280 | 281 | # get bigMs 282 | self._alpha, self._beta = self._get_bigM_dynamics() 283 | self._gamma = self._get_bigM_domains() 284 | 285 | # condense miqp 286 | self.mpmiqp = self._condense_program() 287 | 288 | 289 | def _get_bigM_dynamics(self): 290 | """ 291 | Computes all the bigMs for the dynamics of the PWA system. 292 | The PWA system has the dynamics 293 | x(t+1) = A_i x(t) + B_i u(t) + c_i if (x(t),u(t)) in D_i, 294 | where i in {1, ..., s}. 295 | In order to express it in mixed-integer form, for t = 0, ..., N-1, we introduce the auxiliary variables z_i(t), and we set 296 | x(t+1) = sum_{i=1}^s z_i(t). 297 | We now reformulate the dynamics as 298 | z_i(t) >= alpha_ii delta_i(t), (1) 299 | z_i(t) <= beta_ii delta_i(t), (2) 300 | A_i x(t) + B_i u(t) + c_i - z_i(t) >= sum_{j=1, j!=i}^s alpha_ij delta_j(t), (3) 301 | A_i x(t) + B_i u(t) + c_i - z_i(t) <= sum_{j=1, j!=i}^s beta_ij delta_j(t). (4) 302 | Here alpha_ij (<< 0) and beta_ij (>> 0) are both vectors of bigMs and delta_j(t) is a binary variable (equal to 1 if the system is in mode j, zero otherwise). 303 | If the system is in mode k at time t (i.e. delta_k(t) = 1), we have that 304 | z_i(t) = 0, for all i != k, 305 | z_k(t) >= alpha_kk, 306 | z_k(t) <= beta_kk, 307 | A_i x(t) + B_i u(t) + c_i - z_i(t) >= alpha_ik, for all i != k, 308 | A_i x(t) + B_i u(t) + c_i - z_i(t) <= beta_ik, for all i != k, 309 | A_k x(t) + B_k u(t) + c_k = z_k(t), 310 | that sets 311 | x(t+1) = z_k(t) = A_k x(t) + B_k u(t) + c_k 312 | as desired. 313 | It is very important to choose the bigMs as tight as possible, for this reason we set 314 | alpha_ij := min_{(x,u) in D_j} A_i x + B_i u + c_i, 315 | beta_ij := max_{(x,u) in D_j} A_i x + B_i u + c_i. 316 | (Note that the previous are a number of LPs equal to the number of states.) 317 | The previous ensures that when the system is mode j != i, the dynamics A_i x + B_i u + c_i is lower bounded by alpha_ik and upper bounded by beta_ij. 318 | Returns 319 | ---------- 320 | alpha : list of lists of numpy.ndarray 321 | alpha[i][j] is the vector alpha_ij defined above. 322 | beta : list of lists of numpy.ndarray 323 | beta[i][j] is the vector beta_ij defined above. 324 | """ 325 | 326 | # initialize list of bigMs 327 | alpha = [] 328 | beta = [] 329 | 330 | # outer loop over the number of affine systems 331 | for i, S_i in enumerate(self.S.affine_systems): 332 | alpha_i = [] 333 | beta_i = [] 334 | A_i = np.hstack((S_i.A, S_i.B)) 335 | 336 | # inner loop over the number of affine systems 337 | for j, S_j in enumerate(self.S.affine_systems): 338 | alpha_ij = [] 339 | beta_ij = [] 340 | D_j = self.S.domains[j] 341 | 342 | # solve two LPs for each component of the state vector 343 | for k in range(S_i.nx): 344 | f = A_i[k] 345 | sol = linear_program(f, D_j.A, D_j.b, D_j.C, D_j.d) 346 | alpha_ij.append(sol['min'] + S_i.c[k]) 347 | sol = linear_program(-f, D_j.A, D_j.b, D_j.C, D_j.d) 348 | beta_ij.append(- sol['min'] + S_i.c[k]) 349 | 350 | # close inner loop appending bigMs 351 | alpha_i.append(np.vstack(alpha_ij)) 352 | beta_i.append(np.vstack(beta_ij)) 353 | 354 | # close outer loop appending bigMs 355 | alpha.append(alpha_i) 356 | beta.append(beta_i) 357 | 358 | return alpha, beta 359 | 360 | def _get_bigM_domains(self): 361 | """ 362 | Computes all the bigMs for the domains of the PWA system. 363 | Each one of the s domains of the PWA system has the form 364 | D_i = {(x,u) | F_i x + G_i u <= h_i}. 365 | The bigM reformulation (for t = 0, ..., N-1) of this constraint is 366 | F_i x(t) + G_i u(t) <= h_i + sum_{j=1, j!=i}^s gamma_ij delta_j(t). (5) 367 | Here gamma_ij (>> 0) is a vector of bigMs and delta_j(t) is a binary variable (equal to 1 if the system is in mode j, zero otherwise). 368 | If the system is in mode k at time t (i.e. delta_k(t) = 1), we have that 369 | F_i x(t) + G_i u(t) <= h_i + gamma_ik, for all i != k, 370 | F_k x(t) + G_k u(t) <= h_k, 371 | hence we force (x(t),u(t)) to belong to D_k, whereas the other constraints are redundant because gamma_ik >> 0. 372 | It is very important to choose the bigMs as tight as possible, for this reason we set 373 | gamma_ij := max_{(x,u) in D_j} F_i x + G_i u - h_i. 374 | (Note that the previous are a number of LPs equal to the number of rows of F_i.) 375 | The previous ensures that when the system is mode j != i, gamma_ij is always bigger than the left-hand side (i.e. F_i x + G_i u - h_i). 376 | Returns 377 | ---------- 378 | gamma : list of lists of numpy.ndarray 379 | gamma[i][j] is the vector gamma_ij defined above. 380 | """ 381 | 382 | # initialize list of bigMs 383 | gamma = [] 384 | 385 | # outer loop over the number of affine systems 386 | for i, D_i in enumerate(self.S.domains): 387 | gamma_i = [] 388 | 389 | # inner loop over the number of affine systems 390 | for j, D_j in enumerate(self.S.domains): 391 | gamma_ij = [] 392 | 393 | # solve one LP for each inequality of the ith domain 394 | for k in range(D_i.A.shape[0]): 395 | f = -D_i.A[k] 396 | sol = linear_program(f, D_j.A, D_j.b, D_j.C, D_j.d) 397 | gamma_ij.append(- sol['min'] - D_i.b[k]) 398 | 399 | # close inner loop appending bigMs 400 | gamma_i.append(np.vstack(gamma_ij)) 401 | 402 | # close outer loop appending bigMs 403 | gamma.append(gamma_i) 404 | 405 | return gamma 406 | 407 | def _condense_program(self): 408 | """ 409 | Constructs a multiparametric Mixed Integer Quadratic Program (mpMIQP) in the form 410 | |u|' |Huu 0 0 0| |u| 411 | |z| | 0 Hzz 0 Hzx| |z| 412 | V(x) := min_{u,z,d} 1/2 |d| | 0 0| |d| 413 | |x| |sym Hxx| |x| 414 | s.t. Au u + Az z + Ad d + Ax x <= b 415 | where: 416 | u := (u(0), ..., u(N-1)), continuous, 417 | z := (z(0), ..., z(N-1)), continuous, 418 | d := (d(0), ..., d(N-1)), binary, 419 | while x is the intial condition. 420 | Returns 421 | ---------- 422 | mpmiqp : instance of MultiParametricMixedIntegerQuadraticProgram 423 | Parametric program above. 424 | """ 425 | 426 | # construct blocks and condense constraints 427 | E = self._build_inequalities() 428 | E_bar = self._condense_inequalities(E) 429 | A_bar, Bz_bar = self._condense_equalities() 430 | 431 | # objective of the mpmiqp 432 | H = dict() 433 | H['uu'] = block_diag(*[self.R for i in range(self.N)]) 434 | Q_bar = block_diag(*[self.Q for i in range(self.N)] + [self.P]) 435 | H['zz'] = Bz_bar.T.dot(Q_bar).dot(Bz_bar) 436 | H['zx'] = Bz_bar.T.dot(Q_bar).dot(A_bar) 437 | H['xx'] = A_bar.T.dot(Q_bar).dot(A_bar) 438 | 439 | # constraints of the mpmiqp 440 | A = dict() 441 | A['u'] = E_bar['u'] 442 | A['z'] = E_bar['z'] + E_bar['x'].dot(Bz_bar) 443 | A['d'] = E_bar['d'] 444 | A['x'] = E_bar['x'].dot(A_bar) 445 | b = E_bar['0'] 446 | 447 | return MultiParametricMixedIntegerQuadraticProgram(H, A, b) 448 | 449 | def _build_inequalities(self): 450 | """ 451 | Puts equations (1-4) from the documentation of _get_bigM_dynamics(), equation (5) from the documentation of _get_bigM_domains(), and the condition 452 | sum_{i=1}^s delta_i(t) = 1 (6) 453 | in the form 454 | Ex x(t) + Eu u(t) + Ez z(t) + Ed delta(t) <= E0 455 | where z(t) := (z_1(t), ..., z_s(t)) and delta(t) := (delta_1(t), ..., delta_s(t)). 456 | Returns 457 | ---------- 458 | E : dict of numpy.ndarray 459 | Entries: 'x', 'u', 'z', 'd', '0' (see the defintion above). 460 | """ 461 | 462 | # rename dimensions 463 | nx = self.S.nx 464 | nu = self.S.nu 465 | s = self.S.nm 466 | n_ineq = sum([D.A.shape[0] for D in self.S.domains]) 467 | 468 | # build blocks 469 | E = dict() 470 | E['x'] = np.vstack(( 471 | np.zeros((nx*s, nx)), # Equation 1 472 | np.zeros((nx*s, nx)), # Equation 2 473 | np.vstack([-S.A for S in self.S.affine_systems]), # Equation 3 474 | np.vstack([S.A for S in self.S.affine_systems]), # Equation 4 475 | np.vstack([D.A[:,:nx] for D in self.S.domains]), # Equation 5 476 | np.zeros((2, nx)) # Equation 6 477 | )) 478 | E['u'] = np.vstack(( 479 | np.zeros((nx*s, nu)), # Equation 1 480 | np.zeros((nx*s, nu)), # Equation 2 481 | np.vstack([-S.B for S in self.S.affine_systems]), # Equation 3 482 | np.vstack([S.B for S in self.S.affine_systems]), # Equation 4 483 | np.vstack([D.A[:,nx:] for D in self.S.domains]), # Equation 5 484 | np.zeros((2, nu)) # Equation 6 485 | )) 486 | E['z'] = np.vstack(( 487 | block_diag(*[-np.eye(nx)]*s), # Equation 1 488 | block_diag(*[np.eye(nx)]*s), # Equation 2 489 | block_diag(*[np.eye(nx)]*s), # Equation 3 490 | block_diag(*[-np.eye(nx)]*s), # Equation 4 491 | np.zeros((n_ineq, nx*s)), # Equation 5 492 | np.zeros((2, nx*s)) # Equation 6 493 | )) 494 | E['d'] = np.vstack(( 495 | block_diag(*[self._alpha[i][i] for i in range(s)]), # Equation 1 496 | -block_diag(*[self._beta[i][i] for i in range(s)]), # Equation 2 497 | self._bigM_matrices(self._alpha), # Equation 3 498 | -self._bigM_matrices(self._beta), # Equation 4 499 | -self._bigM_matrices(self._gamma), # Equation 5 500 | np.vstack((np.ones((1, s)), -np.ones((1, s)))) # Equation 6 501 | )) 502 | E['0'] = np.concatenate(( 503 | np.zeros(nx*s), # Equation 1 504 | np.zeros(nx*s), # Equation 2 505 | np.concatenate([S.c for S in self.S.affine_systems]), # Equation 3 506 | np.concatenate([-S.c for S in self.S.affine_systems]), # Equation 4 507 | np.concatenate([D.b for D in self.S.domains]), # Equation 5 508 | np.array([1.,-1.]) # Equation 6 509 | )) 510 | 511 | return E 512 | 513 | @staticmethod 514 | def _bigM_matrices(bigM): 515 | """ 516 | Builds a matrix with the form 517 | | 0 bigM[1][2] bigM[1][3] ...| 518 | |bigM[2][1] 0 bigM[2][3] ...| 519 | |bigM[3][1] bigM[3][2] 0 ...| 520 | | ... ... ... ...| 521 | Arguments 522 | ---------- 523 | bigM : list of lists of numpy.ndarray 524 | bigM[i][j] with i and j in {1, ..., s} is a vector of bigMs. 525 | """ 526 | 527 | # initialize the ouptut with zeros 528 | s = len(bigM) 529 | n_ineq_i = [bigM_i[0].shape[0] for bigM_i in bigM] 530 | mat = np.zeros((sum(n_ineq_i), s)) 531 | 532 | # assemble with bigMs 533 | for i in range(s): 534 | for j in range(s): 535 | if j != i: 536 | mat[sum(n_ineq_i[:i]):sum(n_ineq_i[:i+1]), j:j+1] = bigM[i][j] 537 | 538 | return mat 539 | 540 | def _condense_inequalities(self, E): 541 | """ 542 | Stacks the inequalities 543 | Ex x(t) + Eu u(t) + Ez z(t) + Ed delta(t) <= E0, t = 0, ..., N-1, 544 | x(N) in X_N, 545 | in the form 546 | Ex_bar x_bar + Eu_bar u_bar + Ez_bar z_bar + Ed_bar delta_bar <= E0_bar 547 | where 548 | x_bar := (x(0), ..., x(N)), 549 | u_bar := (u(0), ..., u(N-1)), 550 | z_bar := (z(0), ..., z(N-1)), 551 | delta_bar := (delta(0), ..., delta(N-1)). 552 | Arguments 553 | ---------- 554 | E : dict of numpy.ndarray 555 | Entries: 'x', 'u', 'z', 'd', '0' (see the defintion above). 556 | Returns 557 | ---------- 558 | E_bar : dict of numpy.ndarray 559 | Entries: 'x', 'u', 'z', 'd', '0' (see the defintion above). 560 | """ 561 | 562 | # build blocks 563 | E_bar = dict() 564 | E_bar['x'] = block_diag(*[E['x']]*self.N + [self.X_N.A]) 565 | E_bar['u'] = block_diag(*[E['u']]*self.N) 566 | E_bar['u'] = np.vstack(( 567 | E_bar['u'], 568 | np.zeros((self.X_N.A.shape[0], E_bar['u'].shape[1])) 569 | )) 570 | E_bar['z'] = block_diag(*[E['z']]*self.N) 571 | E_bar['z'] = np.vstack(( 572 | E_bar['z'], 573 | np.zeros((self.X_N.A.shape[0], E_bar['z'].shape[1])) 574 | )) 575 | E_bar['d'] = block_diag(*[E['d']]*self.N) 576 | E_bar['d'] = np.vstack(( 577 | E_bar['d'], 578 | np.zeros((self.X_N.A.shape[0], E_bar['d'].shape[1])) 579 | )) 580 | E_bar['0'] = np.concatenate([E['0']]*self.N + [self.X_N.b]) 581 | 582 | return E_bar 583 | 584 | 585 | def _condense_equalities(self): 586 | """ 587 | Puts the equality constraint 588 | x(0) = x0, 589 | x(t+1) = sum_{i=1}^s z_i(t), t = 0, ..., N-1, 590 | in the form 591 | x_bar = A_bar x0 + Bz_bar z_bar. 592 | Returns 593 | ---------- 594 | A_bar, Bz_bar : numpy.ndarray 595 | See the definition above. 596 | """ 597 | 598 | # build blocks 599 | A_bar = np.vstack(( 600 | np.eye(self.S.nx), 601 | np.zeros((self.S.nx*self.N, self.S.nx)) 602 | )) 603 | Bz_bar = block_diag(*[np.hstack([np.eye(self.S.nx)]*self.S.nm)]*self.N) 604 | Bz_bar = np.vstack(( 605 | np.zeros((self.S.nx, Bz_bar.shape[1])), 606 | Bz_bar 607 | )) 608 | 609 | return A_bar, Bz_bar 610 | 611 | def feedforward(self, x): 612 | """ 613 | Given the state x of the system, returns the optimal sequence of N inputs and the related cost. 614 | Arguments 615 | ---------- 616 | x : numpy.ndarray 617 | State of the system. 618 | Returns 619 | ---------- 620 | u_feedforward : list of numpy.ndarray 621 | Optimal control signals for t = 0, ..., N-1. 622 | V : float 623 | Optimal value function for the given state. 624 | """ 625 | 626 | # solve and check feasibility 627 | sol = self.mpmiqp.solve(x) 628 | if sol['min'] is None: 629 | return None, None, None, None 630 | 631 | # from vector to list of vectors 632 | nu = self.S.nu 633 | nx = self.S.nx 634 | s = self.S.nm 635 | nz = nx*s 636 | u_list = [sol['u'][nu*i:nu*(i+1)] for i in range(self.N)] 637 | z_list = [sol['z'][nz*i:nz*(i+1)] for i in range(self.N)] 638 | d_list = [sol['d'][s*i:s*(i+1)] for i in range(self.N)] 639 | x_list = [x] 640 | for z in z_list: 641 | x_list.append(np.sum([z[nx*i:nx*(i+1)] for i in range(s)], axis=0)) 642 | mode_sequence = [] 643 | for d in d_list: 644 | mode_sequence.append(np.where(d > .5)[0][0]) 645 | 646 | return u_list, x_list, mode_sequence, sol['min'] 647 | 648 | def feedback(self, x): 649 | """ 650 | Returns the optimal feedback for the given state x. 651 | Arguments 652 | ---------- 653 | x : numpy.ndarray 654 | State of the system. 655 | Returns 656 | ---------- 657 | u_feedback : numpy.ndarray 658 | Optimal feedback. 659 | """ 660 | 661 | # get feedforward and extract first input 662 | u_feedforward = self.feedforward(x)[0] 663 | if u_feedforward is None: 664 | return None 665 | 666 | return u_feedforward[0] 667 | 668 | def get_mpqp(self, mode_sequence): 669 | """ 670 | Returns the optimal control problem in condensed form for the given mode sequence. 671 | Arguments 672 | ---------- 673 | mode_sequence : list of int 674 | Sequence of the modes of the PWA system. 675 | Returns 676 | ---------- 677 | instance of MultiParametricQuadraticProgram 678 | Condensed mpQP. 679 | """ 680 | return condense_optimal_control_problem(self.S, self.Q, self.R, self.P, self.X_N, mode_sequence) 681 | 682 | def condense_optimal_control_problem(S, Q, R, P, X_N, mode_sequence): 683 | """ 684 | For a given mode sequences, condenses the optimal control problem for a PWA affine system 685 | min_{x(.), u(.)} 1/2 sum_{t=0}^{N-1} (x'(t) Q x(t) + u'(t) R u(t)) + 1/2 x'(N) P x'(N) 686 | s.t. x(t+1) = A_{z(t)} x(t) + B_{z(t)} u(t) + c_{z(t)} 687 | F_{z(t)} x(t) + G_{z(t)} u(t) <= h_{z(t)} 688 | F_N x(N) <= h_N 689 | where z(t) denotes the mode of the PWA system at time t. 690 | The problem is then stated as a mpQP with parametric initial state x(0). 691 | 692 | Arguments 693 | ---------- 694 | S : instance of PieceWiseAffineSystem 695 | PWA system of the optimal control problem. 696 | Q : numpy.ndarray 697 | Hessian of the state cost. 698 | R : numpy.ndarray 699 | Hessian of the input cost. 700 | P : numpy.ndarray 701 | Hessian of the terminal state cost. 702 | X_N : instance of Polyhedron 703 | Terminal state constraint. 704 | mode_sequence : list of int 705 | Sequence of the modes of the PWA system. 706 | 707 | Returns 708 | ---------- 709 | instance of MultiParametricQuadraticProgram 710 | Condensed mpQP. 711 | """ 712 | 713 | # condense dynamics 714 | A_bar, B_bar, c_bar = S.condense(mode_sequence) 715 | 716 | # stack cost matrices 717 | N = len(mode_sequence) 718 | Q_bar = block_diag(*[Q for i in range(N)] + [P]) 719 | R_bar = block_diag(*[R for i in range(N)]) 720 | 721 | # get blocks quadratic term objective 722 | H = dict() 723 | H['uu'] = R_bar + B_bar.T.dot(Q_bar).dot(B_bar) 724 | H['ux'] = B_bar.T.dot(Q_bar).dot(A_bar) 725 | H['xx'] = A_bar.T.dot(Q_bar).dot(A_bar) 726 | 727 | # get blocks linear term objective 728 | f = dict() 729 | f['u'] = B_bar.T.dot(Q_bar).dot(c_bar) 730 | f['x'] = A_bar.T.dot(Q_bar).dot(c_bar) 731 | g = .5 * c_bar.dot(Q_bar).dot(c_bar) 732 | 733 | # stack constraint matrices 734 | D_sequence = [S.domains[m]for m in mode_sequence] 735 | F_bar = block_diag(*[D.A[:,:S.nx] for D in D_sequence] + [X_N.A]) 736 | G_bar = block_diag(*[D.A[:,S.nx:] for D in D_sequence]) 737 | G_bar = np.vstack(( 738 | G_bar, 739 | np.zeros((X_N.A.shape[0], G_bar.shape[1])) 740 | )) 741 | h_bar = np.concatenate([D.b for D in D_sequence] + [X_N.b]) 742 | 743 | # get blocks for condensed contraints 744 | A = dict() 745 | A['u'] = G_bar + F_bar.dot(B_bar) 746 | A['x'] = F_bar.dot(A_bar) 747 | b = h_bar - F_bar.dot(c_bar) 748 | 749 | return MultiParametricQuadraticProgram(H, f, g, A, b) -------------------------------------------------------------------------------- /pympc/dynamics/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/pympc/dynamics/__init__.py -------------------------------------------------------------------------------- /pympc/dynamics/discrete_time_systems.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import numpy as np 3 | from scipy.linalg import block_diag, solve_discrete_are 4 | from copy import copy 5 | 6 | # internal imports 7 | from pympc.geometry.polyhedron import Polyhedron 8 | from pympc.optimization.programs import linear_program 9 | from pympc.dynamics.discretization_methods import explicit_euler, zero_order_hold 10 | from pympc.dynamics.utils import check_affine_system 11 | 12 | class LinearSystem(object): 13 | """ 14 | Discrete-time linear systems in the form x(t+1) = A x(t) + B u(t). 15 | """ 16 | 17 | def __init__(self, A, B): 18 | """ 19 | Initializes the discrete-time linear system. 20 | 21 | Arguments 22 | ---------- 23 | A : numpy.ndarray 24 | State transition matrix (assumed to be invertible). 25 | B : numpy.ndarray 26 | Input to state map. 27 | """ 28 | 29 | # check inputs 30 | check_affine_system(A, B) 31 | 32 | # store inputs 33 | self.A = A 34 | self.B = B 35 | 36 | # system size 37 | self.nx, self.nu = B.shape 38 | 39 | # property variables 40 | self._controllable = None 41 | 42 | return 43 | 44 | def simulate(self, x0, u): 45 | """ 46 | Simulates the system starting from the state x0 and applying the sequence of controls u. 47 | 48 | Arguments 49 | ---------- 50 | x0 : numpy.ndarray 51 | Initial state. 52 | u : list of numpy.ndarray 53 | Sequence of inputs for t = 0, 1, ..., N-1. 54 | 55 | Returns 56 | ---------- 57 | x : list of numpy.ndarray 58 | Sequence of states for t = 0, 1, ..., N. 59 | """ 60 | 61 | # simulate (do not call condense, too slow) 62 | x = [x0] 63 | for v in u: 64 | x.append(self.A.dot(x[-1]) + self.B.dot(v)) 65 | 66 | return x 67 | 68 | def simulate_closed_loop(self, x0, N, K): 69 | """ 70 | Simulates the system starting from the state x0 for N steps in closed loop with the feedback law u = K x. 71 | 72 | Arguments 73 | ---------- 74 | x0 : numpy.ndarray 75 | Initial state. 76 | K : numpy.ndarray 77 | Feedback gain. 78 | N : int 79 | Number of simulation steps. 80 | 81 | Returns 82 | ---------- 83 | x : list of numpy.ndarray 84 | Sequence of states for t = 0, 1, ..., N. 85 | """ 86 | 87 | # simulate 88 | x = [x0] 89 | for t in range(N): 90 | x.append((self.A + self.B.dot(K)).dot(x[-1])) 91 | 92 | return x 93 | 94 | def solve_dare(self, Q, R): 95 | """ 96 | Returns the solution of the Discrete Algebraic Riccati Equation (DARE). 97 | Consider the linear quadratic control problem V*(x(0)) = min_{x(.), u(.)} 1/2 sum_{t=0}^inf x'(t) Q x(t) + u'(t) R u(t) subject to x(t+1) = A x(t) + B u(t). 98 | The optimal solution is u(0) = K x(0) which leads to V*(x(0)) = 1/2 x'(0) P x(0). 99 | The pair A, B is assumed to be controllable. 100 | 101 | Arguments 102 | ---------- 103 | Q : numpy.ndarray 104 | Quadratic cost for the state (positive semidefinite). 105 | R : numpy.ndarray 106 | Quadratic cost for the input (positive definite). 107 | 108 | Returns 109 | ---------- 110 | P : numpy.ndarray 111 | Hessian of the cost-to-go (positive definite). 112 | K : numpy.ndarray 113 | Optimal feedback gain matrix. 114 | """ 115 | 116 | # check controllability 117 | if not self.controllable: 118 | raise ValueError('uncontrollable system, cannot solve Riccati equation.') 119 | 120 | # cost to go 121 | P = solve_discrete_are(self.A, self.B, Q, R) 122 | 123 | # feedback 124 | K = - np.linalg.inv(self.B.T.dot(P).dot(self.B)+R).dot(self.B.T).dot(P).dot(self.A) 125 | 126 | return P, K 127 | 128 | def mcais(self, K, D, **kwargs): 129 | """ 130 | Returns the maximal constraint-admissible invariant set O_inf for the closed-loop system X(t+1) = (A + B K) x(t). 131 | It holds that x(0) in O_inf <=> (x(t), u(t) = K x(t)) in D for all t >= 0. 132 | 133 | Arguments 134 | ---------- 135 | K : numpy.ndarray 136 | Stabilizing feedback gain for the linear system. 137 | D : instance of Polyhedron 138 | Constraint set in the state and input space. 139 | 140 | Returns 141 | ---------- 142 | O_inf : instance of Polyhedron 143 | Maximal constraint-admissible (positive) ivariant. 144 | t : int 145 | Determinedness index. 146 | """ 147 | 148 | # closed loop dynamics 149 | A_cl = self.A + self.B.dot(K) 150 | 151 | # state-space constraint set 152 | X_cl = Polyhedron( 153 | D.A[:,:self.nx] + D.A[:,self.nx:].dot(K), 154 | D.b 155 | ) 156 | O_inf = mcais(A_cl, X_cl, **kwargs) 157 | 158 | return O_inf 159 | 160 | def condense(self, N): 161 | """ 162 | Constructs the matrices A_bar and B_bar such that x_bar = A_bar x(0) + B_bar u_bar with x_bar = (x(0), ... , x(N)) and u_bar = (u(0), ... , u(N-1)). 163 | 164 | Arguments 165 | ---------- 166 | N : int 167 | Number of time-steps. 168 | 169 | Returns 170 | ---------- 171 | A_bar : numpy.ndarray 172 | Condensed free evolution matrix. 173 | B_bar : numpy.ndarray 174 | Condensed input to state matrix. 175 | """ 176 | 177 | # construct fake affine system 178 | c = np.zeros(self.A.shape[0]) 179 | S = AffineSystem(self.A, self.B, c) 180 | 181 | # condense as if it was a pwa systems 182 | A_bar, B_bar, _ = condense_pwa_system([S], [0]*N) 183 | 184 | return A_bar, B_bar 185 | 186 | @property 187 | def controllable(self): 188 | 189 | # check if already computes 190 | if self._controllable is not None: 191 | return self._controllable 192 | 193 | # check controllability 194 | controllable = False 195 | R = np.hstack([np.linalg.matrix_power(self.A, i).dot(self.B) for i in range(self.nx)]) 196 | self._controllable = np.linalg.matrix_rank(R) == self.nx 197 | 198 | return self._controllable 199 | 200 | @staticmethod 201 | def from_continuous(A, B, h, method='zero_order_hold'): 202 | """ 203 | Instantiates a discrete-time linear system starting from its continuous time representation. 204 | 205 | Arguments 206 | ---------- 207 | A : numpy.ndarray 208 | Continuous-time state transition matrix (assumed to be invertible). 209 | B : numpy.ndarray 210 | Continuous-time state input to state map. 211 | h : float 212 | Discretization time step. 213 | method : str 214 | Discretization method: 'zero_order_hold', or 'explicit_euler'. 215 | """ 216 | 217 | # check inputs 218 | check_affine_system(A, B, None, h) 219 | 220 | # construct affine system 221 | c = np.zeros(A.shape[0]) 222 | 223 | # discretize 224 | if method == 'zero_order_hold': 225 | A_d, B_d, _ = zero_order_hold(A, B, c, h) 226 | elif method == 'explicit_euler': 227 | A_d, B_d, _ = explicit_euler(A, B, c, h) 228 | else: 229 | raise ValueError('unknown discretization method.') 230 | 231 | return LinearSystem(A_d, B_d) 232 | 233 | @staticmethod 234 | def from_symbolic(x, u, x_next): 235 | """ 236 | Instatiates a LinearSystem starting from the symbolic value of the next state. 237 | 238 | Arguments 239 | ---------- 240 | x : sympy matrix filled with sympy symbols 241 | Symbolic state of the system. 242 | u : sympy matrix filled with sympy symbols 243 | Symbolic input of the system. 244 | x_next : sympy matrix filled with sympy symbolic linear expressions 245 | Symbolic value of the state update. 246 | """ 247 | 248 | # state transition matrices 249 | A, B, c = get_state_transition_matrices(x, u, x_next) 250 | 251 | # check that offset setm is zero 252 | if not np.allclose(c, np.zeros(x.shape[0])): 253 | raise ValueError('The given system has a non zero offset.') 254 | 255 | return LinearSystem(A, B) 256 | 257 | @staticmethod 258 | def from_symbolic_continuous(x, u, x_dot, h, method='zero_order_hold'): 259 | """ 260 | Instatiates a LinearSystem starting from the symbolic value of the next state. 261 | 262 | Arguments 263 | ---------- 264 | x : sympy matrix filled with sympy symbols 265 | Symbolic state of the system. 266 | u : sympy matrix filled with sympy symbols 267 | Symbolic input of the system. 268 | x_dot : sympy matrix filled with sympy symbolic linear expressions 269 | Symbolic value of the state time derivative. 270 | h : float 271 | Discretization time step. 272 | method : str 273 | Discretization method: 'zero_order_hold', or 'explicit_euler'. 274 | """ 275 | 276 | # state transition matrices 277 | A, B, c = get_state_transition_matrices(x, u, x_dot) 278 | 279 | # check that offset setm is zero 280 | if not np.allclose(c, np.zeros(x.shape[0])): 281 | raise ValueError('The given system has a non zero offset.') 282 | 283 | return LinearSystem.from_continuous(A, B, h, method) 284 | 285 | class AffineSystem(object): 286 | """ 287 | Discrete-time affine systems in the form x(t+1) = A x(t) + B u(t) + c. 288 | """ 289 | 290 | def __init__(self, A, B, c): 291 | """ 292 | Initializes the discrete-time affine system. 293 | 294 | Arguments 295 | ---------- 296 | A : numpy.ndarray 297 | State transition matrix (assumed to be invertible). 298 | B : numpy.ndarray 299 | Input to state map. 300 | c : numpy.ndarray 301 | Offset term in the dynamics. 302 | """ 303 | 304 | # check inputs 305 | check_affine_system(A, B, c) 306 | 307 | # store inputs 308 | self.A = A 309 | self.B = B 310 | self.c = c 311 | 312 | # system size 313 | self.nx, self.nu = B.shape 314 | 315 | def simulate(self, x0, u): 316 | """ 317 | Simulates the system starting from the state x0 and applying the sequence of controls u. 318 | 319 | Arguments 320 | ---------- 321 | x0 : numpy.ndarray 322 | Initial state. 323 | u : list of numpy.ndarray 324 | Sequence of inputs for t = 0, 1, ..., N-1. 325 | 326 | Returns 327 | ---------- 328 | x : list of numpy.ndarray 329 | Sequence of states for t = 0, 1, ..., N. 330 | """ 331 | 332 | # simulate (do not call condense, too slow) 333 | x = [x0] 334 | for v in u: 335 | x.append(self.A.dot(x[-1]) + self.B.dot(v) + self.c) 336 | 337 | return x 338 | 339 | def condense(self, N): 340 | """ 341 | Constructs the matrices A_bar, B_bar cnd c_bar such that x_bar = A_bar x(0) + B_bar u_bar + c_bar with x_bar = (x(0), ... , x(N)) and u_bar = (u(0), ... , u(N-1)). 342 | 343 | Arguments 344 | ---------- 345 | N : int 346 | Number of time-steps. 347 | 348 | Returns 349 | ---------- 350 | A_bar : numpy.ndarray 351 | Condensed free evolution matrix. 352 | B_bar : numpy.ndarray 353 | Condensed input to state matrix. 354 | c_bar : numpy.ndarray 355 | Condensed offset term matrix. 356 | """ 357 | 358 | # call the function for pwa systems with fake pwa system 359 | return condense_pwa_system([self], [0]*N) 360 | 361 | @staticmethod 362 | def from_continuous(A, B, c, h, method='zero_order_hold'): 363 | """ 364 | Instantiates a discrete-time affine system starting from its continuous time representation. 365 | 366 | Arguments 367 | ---------- 368 | A : numpy.ndarray 369 | Continuous-time state transition matrix (assumed to be invertible). 370 | B : numpy.ndarray 371 | Continuous-time state input to state map. 372 | c : numpy.ndarray 373 | Offset term in the dynamics. 374 | h : float 375 | Discretization time step. 376 | method : str 377 | Discretization method: 'zero_order_hold', or 'explicit_euler'. 378 | """ 379 | 380 | # check inputs 381 | check_affine_system(A, B, c, h) 382 | 383 | # discretize 384 | if method == 'zero_order_hold': 385 | A_d, B_d, c_d = zero_order_hold(A, B, c, h) 386 | elif method == 'explicit_euler': 387 | A_d, B_d, c_d = explicit_euler(A, B, c, h) 388 | else: 389 | raise ValueError('unknown discretization method.') 390 | 391 | return AffineSystem(A_d, B_d, c_d) 392 | 393 | @staticmethod 394 | def from_symbolic(x, u, x_next): 395 | """ 396 | Instatiates a AffineSystem starting from the symbolic value of the next state. 397 | 398 | Arguments 399 | ---------- 400 | x : sympy matrix filled with sympy symbols 401 | Symbolic state of the system. 402 | u : sympy matrix filled with sympy symbols 403 | Symbolic input of the system. 404 | x_next : sympy matrix filled with sympy symbolic linear expressions 405 | Symbolic value of the state update. 406 | """ 407 | 408 | return AffineSystem(*get_state_transition_matrices(x, u, x_next)) 409 | 410 | @staticmethod 411 | def from_symbolic_continuous(x, u, x_dot, h, method='zero_order_hold'): 412 | """ 413 | Instatiates a LinearSystem starting from the symbolic value of the next state. 414 | 415 | Arguments 416 | ---------- 417 | x : sympy matrix filled with sympy symbols 418 | Symbolic state of the system. 419 | u : sympy matrix filled with sympy symbols 420 | Symbolic input of the system. 421 | x_dot : sympy matrix filled with sympy symbolic linear expressions 422 | Symbolic value of the state time derivative. 423 | h : float 424 | Discretization time step. 425 | method : str 426 | Discretization method: 'zero_order_hold', or 'explicit_euler'. 427 | """ 428 | 429 | # get state transition matrices 430 | A, B, c = get_state_transition_matrices(x, u, x_dot) 431 | 432 | return AffineSystem.from_continuous(A, B, c, h, method) 433 | 434 | class PieceWiseAffineSystem(object): 435 | """ 436 | Discrete-time piecewise-affine systems in the form x(t+1) = A_i x(t) + B_i u(t) + c_i if (x(t), u(t)) in D_i := {(x,u) | F_i x + G_i u <= h_i}. 437 | """ 438 | 439 | def __init__(self, affine_systems, domains): 440 | """ 441 | Initializes the discrete-time piecewise-affine system. 442 | 443 | Arguments 444 | ---------- 445 | affine_systems : list of instances of AffineSystem 446 | List of the dynamics for each mode of the system (in case a LinearSystem is passed through this list it is automatically converted to an instance of AffineSystem). 447 | domains : list of instances of Polyhedron 448 | Domains of each mode of the system. 449 | """ 450 | 451 | # same number of systems and domains 452 | if len(affine_systems) != len(domains): 453 | raise ValueError('the number of affine systems has to be equal to the number of domains.') 454 | 455 | # same number of states for each system 456 | nx = set(S.nx for S in affine_systems) 457 | if len(nx) != 1: 458 | raise ValueError('all the affine systems must have the same number of states.') 459 | self.nx = list(nx)[0] 460 | 461 | # same number of inputs for each system 462 | nu = set(S.nu for S in affine_systems) 463 | if len(nu) != 1: 464 | raise ValueError('all the affine systems must have the same number of inputs.') 465 | self.nu = list(nu)[0] 466 | 467 | # same dimensions for each domain 468 | nxu = set(D.A.shape[1] for D in domains) 469 | if len(nxu) != 1: 470 | raise ValueError('all the domains must have equal dimnesionality.') 471 | 472 | # dimension of each domain equal too number of states plus number of inputs 473 | if list(nxu)[0] != self.nx + self.nu: 474 | raise ValueError('the domains and the affine systems must have coherent dimensions.') 475 | 476 | # make instances of LinearSystem instances of AffineSystem 477 | for i, S in enumerate(affine_systems): 478 | if isinstance(S, LinearSystem): 479 | c = np.zeros(self.nx) 480 | affine_systems[i] = AffineSystem(S.A, S.B, c) 481 | 482 | # store inputs 483 | self.affine_systems = affine_systems 484 | self.domains = domains 485 | self.nm = len(affine_systems) 486 | 487 | def condense(self, mode_sequence): 488 | """ 489 | See the documentation of condense_pwa_system(). 490 | """ 491 | return condense_pwa_system(self.affine_systems, mode_sequence) 492 | 493 | def simulate(self, x0, u): 494 | """ 495 | Given the initial state x0 and a list of inputs, simulates the PWA dynamics. 496 | If the couple (x(t), u(t)) goes out of the domains D_i raises a ValueError. 497 | 498 | Arguments 499 | ---------- 500 | x0 : numpy.ndarray 501 | Initial state. 502 | u : list of numpy.ndarray 503 | Sequence of inputs for t = 0, 1, ..., N-1. 504 | 505 | Returns 506 | ---------- 507 | x : list of numpy.ndarray 508 | Sequence of states for t = 0, 1, ..., N. 509 | mode_sequence : list of int 510 | Sequence of the modes that the PWA system is in for time t = 0, 1, ..., N-1. 511 | """ 512 | 513 | # initialize output 514 | x = [x0] 515 | mode_sequence = [] 516 | 517 | # simulate 518 | for t in range(len(u)): 519 | mode = self.get_mode(x[t], u[t]) 520 | 521 | # if outside the domain, raise value error 522 | if mode is None: 523 | raise ValueError('simulation reached an unfeasible point x = ' + str(x[t]) + ', u = ' + str(u[t]) + '.') 524 | 525 | # compute next state and append values 526 | else: 527 | S = self.affine_systems[mode] 528 | x.append(S.A.dot(x[t]) + S.B.dot(u[t]) + S.c) 529 | mode_sequence.append(mode) 530 | 531 | return x, mode_sequence 532 | 533 | def get_mode(self, x, u): 534 | """ 535 | Given (x,u) returns the i such that (x,u) in D_i. 536 | 537 | Arguments 538 | ---------- 539 | x : numpy.ndarray 540 | Value for the state vector. 541 | u : numpy.ndarray 542 | Value for the input vector. 543 | 544 | Returns 545 | ---------- 546 | i : int 547 | Index of the mode of the system (None ix there is no domain that contains (x,u)). 548 | """ 549 | 550 | # loop over the domains 551 | xu = np.concatenate((x, u)) 552 | for i, D in enumerate(self.domains): 553 | if D.contains(xu): 554 | return i 555 | 556 | return None 557 | 558 | def is_well_posed(self, tol=1.e-7): 559 | """ 560 | Check if the domains of the pwa system are well posed (i.e. if the intersection of the interior of D_i with the interior of D_j is empty for all i and j != i). 561 | 562 | Arguments 563 | ---------- 564 | tol : float 565 | Maximum penetration of two sets to assume that their interiors do not intersect. 566 | 567 | Returns: 568 | ---------- 569 | well_posed : bool 570 | True if the domains are well posed, False otherwise. 571 | """ 572 | 573 | # loop over al the combinations (avoiding to check twice) 574 | for i, Di in enumerate(self.domains): 575 | for j in range(i+1, self.nm): 576 | Dij = Di.intersection(self.domains[j]) 577 | 578 | # check the Chebyshev radius of the intersection 579 | if Dij.radius > tol: 580 | return False 581 | 582 | return True 583 | 584 | def mcais(A, X, verbose=False): 585 | """ 586 | Returns the maximal constraint-admissible (positive) invariant set O_inf for the system x(t+1) = A x(t) subject to the constraint x in X. 587 | O_inf is also known as maximum output admissible set. 588 | It holds that x(0) in O_inf <=> x(t) in X for all t >= 0. 589 | (Implementation of Algorithm 3.2 from: Gilbert, Tan - Linear Systems with State and Control Constraints, The Theory and Application of Maximal Output Admissible Sets.) 590 | Sufficient conditions for this set to be finitely determined (i.e. defined by a finite number of facets) are: A stable, X bounded and containing the origin. 591 | 592 | Math 593 | ---------- 594 | At each time step t, we want to verify if at the next time step t+1 the system will go outside X. 595 | Let's consider X := {x | D_i x <= e_i, i = 1,...,n} and t = 0. 596 | In order to ensure that x(1) = A x(0) is inside X, we need to consider one by one all the constraints and for each of them, the worst-case x(0). 597 | We can do this solvin an LP 598 | V(t=0, i) = max_{x in X} D_i A x - e_i for i = 1,...,n 599 | if all these LPs has V < 0 there is no x(0) such that x(1) is outside X. 600 | The previous implies that all the time-evolution x(t) will lie in X (see Gilbert and Tan). 601 | In case one of the LPs gives a V > 0, we iterate and consider 602 | V(t=1, i) = max_{x in X, x in A X} D_i A^2 x - e_i for i = 1,...,n 603 | where A X := {x | D A x <= e}. 604 | If now all V < 0, then O_inf = X U AX, otherwise we iterate until convergence 605 | V(t, i) = max_{x in X, x in A X, ..., x in A^t X} D_i A^(t+1) x - e_i for i = 1,...,n 606 | Once at convergence O_Inf = X U A X U ... U A^t X. 607 | 608 | Arguments 609 | ---------- 610 | A : numpy.ndarray 611 | State transition matrix. 612 | X : instance of Polyhedron 613 | State-space domain of the dynamical system. 614 | verbose : bool 615 | If True prints at each iteration the convergence parameters. 616 | 617 | Returns: 618 | ---------- 619 | O_inf : instance of Polyhedron 620 | Maximal constraint-admissible (positive) ivariant. 621 | t : int 622 | Determinedness index. 623 | """ 624 | 625 | # ensure convergence of the algorithm 626 | eig_max = np.max(np.absolute(np.linalg.eig(A)[0])) 627 | if eig_max > 1.: 628 | raise ValueError('unstable system, cannot derive maximal constraint-admissible set.') 629 | [nc, nx] = X.A.shape 630 | if not X.contains(np.zeros((nx, 1))): 631 | raise ValueError('the origin is not contained in the constraint set, cannot derive maximal constraint-admissible set.') 632 | if not X.bounded: 633 | raise ValueError('unbounded constraint set, cannot derive maximal constraint-admissible set.') 634 | 635 | # initialize mcais 636 | O_inf = copy(X) 637 | 638 | # loop over time 639 | t = 1 640 | convergence = False 641 | while not convergence: 642 | 643 | # solve one LP per facet 644 | J = X.A.dot(np.linalg.matrix_power(A,t)) 645 | residuals = [] 646 | for i in range(X.A.shape[0]): 647 | sol = linear_program(- J[i], O_inf.A, O_inf.b) 648 | residuals.append(- sol['min'] - X.b[i]) 649 | 650 | # print status of the algorithm 651 | if verbose: 652 | print('Time horizon: ' + str(t) + '.'), 653 | print('Convergence index: ' + str(max(residuals)) + '.'), 654 | print('Number of facets: ' + str(O_inf.A.shape[0]) + '. \r'), 655 | 656 | # convergence check 657 | new_facets = [i for i, r in enumerate(residuals) if r > 0.] 658 | if len(new_facets) == 0: 659 | convergence = True 660 | else: 661 | 662 | # add (only non-redundant!) facets 663 | O_inf.add_inequality(J[new_facets], X.b[new_facets]) 664 | t += 1 665 | 666 | # remove redundant facets 667 | if verbose: 668 | print('\nMaximal constraint-admissible invariant set found.') 669 | print('Removing redundant facets ...'), 670 | O_inf.remove_redundant_inequalities() 671 | if verbose: 672 | print('minimal facets are ' + str(O_inf.A.shape[0]) + '.') 673 | 674 | return O_inf 675 | 676 | def condense_pwa_system(affine_systems, mode_sequence): 677 | """ 678 | For the PWA system 679 | x(t+1) = A_i x(t) + B_i u(t) + c_i if (x(t), u(t)) in D_i, 680 | given the mode sequence z = (z(0), ... , z(N-1)), returns the matrices A_bar, B_bar, c_bar such that 681 | x_bar = A_bar x(0) + B_bar u_bar + c_bar 682 | with x_bar = (x(0), ... , x(N)) and u_bar = (u(0), ... , u(N-1)). 683 | 684 | Arguments 685 | ---------- 686 | affine_systems : list of instances of AffineSystem 687 | State transition matrix (assumed to be invertible). 688 | mode_sequence : list of int 689 | Sequence of the modes that the PWA system is in for time t = 0, 1, ..., N-1. 690 | 691 | Returns 692 | ---------- 693 | A_bar : numpy.ndarray 694 | Condensed free evolution matrix. 695 | B_bar : numpy.ndarray 696 | Condensed input to state matrix. 697 | c_bar : numpy.ndarray 698 | Condensed offset term matrix. 699 | """ 700 | 701 | # system dimensions 702 | nx = affine_systems[0].nx 703 | nu = affine_systems[0].nu 704 | N = len(mode_sequence) 705 | 706 | # matrix sequence 707 | A_sequence = [affine_systems[mode_sequence[i]].A for i in range(N)] 708 | B_sequence = [affine_systems[mode_sequence[i]].B for i in range(N)] 709 | c_sequence = [affine_systems[mode_sequence[i]].c for i in range(N)] 710 | 711 | # free evolution of the system 712 | A_bar = np.vstack([productory(A_sequence[i::-1]) for i in range(N)]) 713 | A_bar = np.vstack((np.eye(nx), A_bar)) 714 | 715 | # forced evolution of the system 716 | B_bar = np.zeros((nx*N,nu*N)) 717 | for i in range(N): 718 | for j in range(i): 719 | B_bar[nx*i:nx*(i+1), nu*j:nu*(j+1)] = productory(A_sequence[i:j:-1]).dot(B_sequence[j]) 720 | B_bar[nx*i:nx*(i+1), nu*i:nu*(i+1)] = B_sequence[i] 721 | B_bar = np.vstack((np.zeros((nx, nu*N)), B_bar)) 722 | 723 | # evolution related to the offset term 724 | c_bar = np.concatenate((np.zeros(nx), c_sequence[0])) 725 | for i in range(1, N): 726 | offset_i = sum([productory(A_sequence[i:j:-1]).dot(c_sequence[j]) for j in range(i)]) + c_sequence[i] 727 | c_bar = np.concatenate((c_bar, offset_i)) 728 | 729 | return A_bar, B_bar, c_bar 730 | 731 | def productory(matrix_list): 732 | """ 733 | Multiplies from lest to right the matrices in the list matrix_list. 734 | 735 | Arguments 736 | ---------- 737 | matrix_list : list of numpy.ndarray 738 | List of matrices to be multiplied. 739 | 740 | Returns 741 | ---------- 742 | M : numpy.ndarray 743 | Product of the matrices in matrix_list. 744 | """ 745 | 746 | # start wiht the first elment and multy all the others 747 | A = matrix_list[0] 748 | for B in matrix_list[1:]: 749 | A = A.dot(B) 750 | 751 | return A 752 | 753 | def get_state_transition_matrices(x, u, x_next): 754 | """ 755 | Extracts from the symbolic expression of the state at the next time step the matrices A, B, and c. 756 | Arguments 757 | ---------- 758 | x : sympy matrix filled with sympy symbols 759 | Symbolic state of the system. 760 | u : sympy matrix filled with sympy symbols 761 | Symbolic input of the system. 762 | x_next : sympy matrix filled with sympy symbolic linear expressions 763 | Symbolic value of the state update. 764 | """ 765 | 766 | # state transition matrices 767 | A = np.array(x_next.jacobian(x)).astype(np.float64) 768 | B = np.array(x_next.jacobian(u)).astype(np.float64) 769 | 770 | # offset term 771 | origin = {xi:0 for xi in x} 772 | origin.update({ui:0 for ui in u}) 773 | c = np.array(x_next.subs(origin)).astype(np.float64).flatten() 774 | 775 | return A, B, c -------------------------------------------------------------------------------- /pympc/dynamics/discretization_methods.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import numpy as np 3 | from scipy.linalg import expm 4 | 5 | # internal imports 6 | from pympc.dynamics.utils import check_affine_system 7 | 8 | def explicit_euler(A, B, c, h): 9 | """ 10 | Discretizes the continuous-time affine system dx/dt = A x + B u + c approximating x(t+1) with x(t) + h dx/dt(t). 11 | 12 | Arguments 13 | ---------- 14 | A : numpy.ndarray 15 | State transition matrix. 16 | B : numpy.ndarray 17 | Input to state map. 18 | c : numpy.ndarray 19 | Offset term. 20 | h : float 21 | Discretization time step. 22 | 23 | Returns 24 | ---------- 25 | A_d : numpy.ndarray 26 | Discrete-time state transition matrix. 27 | B_d : numpy.ndarray 28 | Discrete-time input to state map. 29 | c_d : numpy.ndarray 30 | Discrete-time offset term. 31 | """ 32 | 33 | # check inputs 34 | check_affine_system(A, B, c, h) 35 | 36 | # discretize 37 | A_d = A*h + np.eye(A.shape[0]) 38 | B_d = B*h 39 | c_d = c*h 40 | 41 | return A_d, B_d, c_d 42 | 43 | def zero_order_hold(A, B, c, h): 44 | """ 45 | Assuming piecewise constant inputs, it returns the exact discretization of the affine system dx/dt = A x + B u + c. 46 | 47 | Math 48 | ---------- 49 | Solving the differential equation, we have 50 | x(h) = exp(A h) x(0) + int_0^h exp(A (h - t)) (B u(t) + c) dt. 51 | Being u(t) = u(0) constant between 0 and h we have 52 | x(h) = A_d x(0) + B_d u(0) + c_d, 53 | where 54 | A_d := exp(A h), 55 | B_d := int_0^h exp(A (h - t)) dt B, 56 | c_d = B_d := int_0^h exp(A (h - t)) dt c. 57 | I holds 58 | |A B c| |A_d B_d c_d| 59 | exp (|0 0 0| h) = |0 I 0 | 60 | |0 0 0| |0 0 1 | 61 | where both the matrices are square. 62 | Proof: apply the definition of exponential and note that int_0^h exp(A (h - t)) dt = sum_{k=1}^inf A^(k-1) h^k/k!. 63 | 64 | Arguments 65 | ---------- 66 | A : numpy.ndarray 67 | State transition matrix. 68 | B : numpy.ndarray 69 | Input to state map. 70 | c : numpy.ndarray 71 | Offset term. 72 | h : float 73 | Discretization time step. 74 | 75 | Returns 76 | ---------- 77 | A_d : numpy.ndarray 78 | Discrete-time state transition matrix. 79 | B_d : numpy.ndarray 80 | Discrete-time input to state map. 81 | c_d : numpy.ndarray 82 | Discrete-time offset term. 83 | """ 84 | 85 | # check inputs 86 | check_affine_system(A, B, c, h) 87 | 88 | # system dimensions 89 | n_x = np.shape(A)[0] 90 | n_u = np.shape(B)[1] 91 | 92 | # zero order hold 93 | M_c = np.vstack(( 94 | np.column_stack((A, B, c)), 95 | np.zeros((n_u+1, n_x+n_u+1)) 96 | )) 97 | M_d = expm(M_c * h) 98 | 99 | # discrete time dynamics 100 | A_d = M_d[:n_x, :n_x] 101 | B_d = M_d[:n_x, n_x:n_x+n_u] 102 | c_d = M_d[:n_x, n_x+n_u] 103 | 104 | return A_d, B_d, c_d -------------------------------------------------------------------------------- /pympc/dynamics/utils.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import numpy as np 3 | 4 | def check_affine_system(A, B, c=None, h=None): 5 | """ 6 | Check that the matrices A, B, and c of an affine system have compatible sizes. 7 | 8 | Arguments 9 | ---------- 10 | A : numpy.ndarray 11 | State transition matrix. 12 | B : numpy.ndarray 13 | Input to state map. 14 | c : numpy.ndarray 15 | Offset term. 16 | h : float 17 | Discretization time step. 18 | """ 19 | 20 | # A square matrix 21 | if A.shape[0] != A.shape[1]: 22 | raise ValueError('A must be a square matrix.') 23 | 24 | # equal number of rows for A and B 25 | if A.shape[0] != B.shape[0]: 26 | raise ValueError('A and B must have the same number of rows.') 27 | 28 | # check c 29 | if c is not None: 30 | if c.ndim > 1: 31 | raise ValueError('c must be a 1-dimensional array.') 32 | if A.shape[0] != c.size: 33 | raise ValueError('A and c must have the same number of rows.') 34 | 35 | # check h 36 | if h is not None: 37 | if h < 0: 38 | raise ValueError('the time step h must be positive.') -------------------------------------------------------------------------------- /pympc/geometry/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/pympc/geometry/__init__.py -------------------------------------------------------------------------------- /pympc/geometry/utils.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import numpy as np 3 | 4 | def nullspace_basis(A): 5 | """ 6 | Uses SVD to find a basis of the nullsapce of A. 7 | 8 | Arguments 9 | ---------- 10 | A : numpy.ndarray 11 | Matrix for the nullspace. 12 | 13 | Returns 14 | ---------- 15 | Z : numpy.ndarray 16 | Nullspace basis. 17 | """ 18 | 19 | # get singular values 20 | V = np.linalg.svd(A)[2].T 21 | 22 | # cut to the dimension of the rank 23 | rank = np.linalg.matrix_rank(A) 24 | Z = V[:,rank:] 25 | 26 | return Z 27 | 28 | def linearly_independent_rows(A, tol=1.e-6): 29 | """ 30 | uses the QR decomposition to find the indices of a set of linear independent rows of the matrix A. 31 | 32 | Arguments 33 | ---------- 34 | A : numpy.ndarray 35 | Matrix for the linear independent rows. 36 | tol : float 37 | Threshold value for the diagonal elements of R. 38 | 39 | Returns 40 | ---------- 41 | independent_rows : list of int 42 | List of indices of a set of independent rows of A. 43 | """ 44 | 45 | # QR decomposition 46 | R = np.linalg.qr(A.T)[1] 47 | 48 | # check diagonal elements 49 | R_diag = np.abs(np.diag(R)) 50 | independent_rows = list(np.where(R_diag > tol)[0]) 51 | 52 | return sorted(independent_rows) 53 | 54 | def plane_through_points(points): 55 | """ 56 | Returns the plane a' x = b passing through the points. 57 | It first adds a random offset to be sure that the matrix of the points is invertible (it wouldn't be the case if the plane we are looking for passes through the origin). 58 | The vector a has norm equal to one and b is non-negative. 59 | 60 | Arguments 61 | ---------- 62 | points : list of numpy.ndarray 63 | List of points that the plane has to fit. 64 | 65 | Returns 66 | ---------- 67 | a : numpy.ndarray 68 | Left-hand side of the equality describing the plane. 69 | d : numpy.ndarray 70 | Right-hand side of the equality describing the plane. 71 | """ 72 | 73 | # generate random offset 74 | offset = np.random.rand(points[0].size) 75 | points = [p + offset for p in points] 76 | 77 | # solve linear system 78 | P = np.vstack(points) 79 | a = np.linalg.solve(P, np.ones(points[0].size)) 80 | 81 | # go back to the original coordinates 82 | d = 1. - a.dot(offset) 83 | 84 | # scale and select sign of the result 85 | a_norm = np.linalg.norm(a) 86 | d_sign = np.sign(d) 87 | if d_sign == 0.: 88 | d_sign = 1. 89 | a /= a_norm * d_sign 90 | d /= a_norm * d_sign 91 | 92 | return a, d 93 | 94 | def same_rows(A, B, normalize=True): 95 | """ 96 | Checks if two matrices contain the same rows. 97 | The order of the rows can be different. 98 | The option normalize, normalizes the rows of A and B; i.e., if True, checks that set of rows of A is the same of the one of B despite a scaling factor. 99 | 100 | Arguments 101 | ---------- 102 | A : numpy.ndarray 103 | First matrix to check. 104 | B : numpy.ndarray 105 | Second matrix to check. 106 | normalize : bool 107 | If True scales the rows of A and B to have norm equal to one. 108 | 109 | Returns: 110 | equal : bool 111 | True if the set of rows of A and B are the same. 112 | """ 113 | 114 | # first check the sizes 115 | if A.shape[0] != B.shape[0]: 116 | return False 117 | 118 | # if required, normalize 119 | if normalize: 120 | for i in range(A.shape[0]): 121 | A[i] = A[i] / np.linalg.norm(A[i]) 122 | B[i] = B[i] / np.linalg.norm(B[i]) 123 | 124 | # check one row per time 125 | for a in A: 126 | i = np.where([np.allclose(a, b) for b in B])[0] 127 | if len(i) != 1: 128 | return False 129 | B = np.delete(B, i, 0) 130 | 131 | return True 132 | 133 | def same_vectors(v_list, u_list): 134 | """ 135 | Tests that two lists of array contain the same elements. 136 | The order of the elements in the lists can be different. 137 | 138 | Arguments 139 | ---------- 140 | v_list : list of numpy.ndarray 141 | First ist of arrays to be checked. 142 | u_list : list of numpy.ndarray 143 | Second ist of arrays to be checked. 144 | 145 | Returns: 146 | equal : bool 147 | True if the set of arrays oin v_list and u_list are the same. 148 | """ 149 | 150 | # check inputs 151 | for z_list in [v_list, u_list]: 152 | if any(len(z.shape) >= 2 for z in z_list): 153 | raise ValueError('input vectors must be 1-dimensional arrays.') 154 | 155 | # construct matrices 156 | V = np.vstack(v_list) 157 | U = np.vstack(u_list) 158 | 159 | return same_rows(V, U, False) -------------------------------------------------------------------------------- /pympc/optimization/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/pympc/optimization/__init__.py -------------------------------------------------------------------------------- /pympc/optimization/parametric_programs.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import numpy as np 3 | from scipy.linalg import block_diag 4 | from copy import copy 5 | 6 | # internal inputs 7 | from pympc.geometry.polyhedron import Polyhedron 8 | from pympc.optimization.programs import quadratic_program, mixed_integer_quadratic_program 9 | 10 | class MultiParametricQuadraticProgram(object): 11 | """ 12 | mpQP in the form 13 | |u|' |Huu Hux| |u| |fu|' |u| 14 | V(x) := min_u 1/2 |x| |Hux' Hxx| |x| + |fx| |x| + g 15 | s.t. Au u + Ax x <= b 16 | """ 17 | 18 | def __init__(self, H, f, g, A, b): 19 | """ 20 | Instantiates the parametric mpQP. 21 | 22 | Arguments 23 | ---------- 24 | H : dict of numpy.ndarray 25 | Blocks of the quaratic term, keys: 'xx', 'ux', 'xx'. 26 | f : dict of numpy.ndarray 27 | Blocks of the linear term, keys: 'x', 'u'. 28 | g : numpy.ndarray 29 | Offset term in the cost function. 30 | A : dict of numpy.ndarray 31 | Left-hand side of the constraints, keys: 'x', 'u'. 32 | b : numpy.ndarray 33 | Right-hand side of the constraints. 34 | """ 35 | self.H = H 36 | self.Huu_inv = np.linalg.inv(self.H['uu']) 37 | self.f = f 38 | self.g = g 39 | self.A = A 40 | self.b = b 41 | 42 | def explicit_solve_given_active_set(self, active_set): 43 | """ 44 | Returns the explicit solution of the mpQP for a given active set. 45 | The solution turns out to be an affine function of x, i.e. u(x) = ux x + u0, p(x) = px x + p0, where p are the Lagrange multipliers for the inequality constraints. 46 | 47 | Math 48 | ---------- 49 | Given an active set A and a set of multipliers p, the KKT conditions for the mpQP are a set of linear equations that can be solved for u(x) and p(x) 50 | Huu u + Hux + fu + Aua' pa = 0, (stationarity of the Lagrangian), 51 | Aua u + Axa x = ba, (primal feasibility), 52 | pi = 0, (dual feasibility), 53 | where the subscripts a and i dentote active and inactive inequalities respectively. 54 | The inactive (primal and dual) constraints define the region of space where the given active set is optimal 55 | Aui u + Axi x < bi, (primal feasibility), 56 | pa > 0, (dual feasibility). 57 | 58 | 59 | Arguments 60 | ---------- 61 | active_set : list of int 62 | Indices of the active inequalities. 63 | 64 | Reuturns 65 | ---------- 66 | instance of CriticalRegion 67 | Critical region for the given active set. 68 | """ 69 | 70 | # ensure that LICQ will hold 71 | Aua = self.A['u'][active_set] 72 | if len(active_set) > 0 and np.linalg.matrix_rank(Aua) < Aua.shape[0]: 73 | return None 74 | 75 | # split active and inactive 76 | inactive_set = [i for i in range(self.A['x'].shape[0]) if i not in active_set] 77 | Aui = self.A['u'][inactive_set] 78 | Axa = self.A['x'][active_set] 79 | Axi = self.A['x'][inactive_set] 80 | ba = self.b[active_set] 81 | bi = self.b[inactive_set] 82 | 83 | # multipliers 84 | M = np.linalg.inv(Aua.dot(self.Huu_inv).dot(Aua.T)) 85 | pax = M.dot(Axa - Aua.dot(self.Huu_inv).dot(self.H['ux'])) 86 | pa0 = - M.dot(ba + Aua.dot(self.Huu_inv).dot(self.f['u'])) 87 | px = np.zeros(self.A['x'].shape) 88 | p0 = np.zeros(self.A['x'].shape[0]) 89 | px[active_set] = pax 90 | p0[active_set] = pa0 91 | p = {'x': px, '0':p0} 92 | 93 | # primary variables 94 | ux = - self.Huu_inv.dot(self.H['ux'] + Aua.T.dot(pax)) 95 | u0 = - self.Huu_inv.dot(self.f['u'] + Aua.T.dot(pa0)) 96 | u = {'x':ux, '0':u0} 97 | 98 | # critical region 99 | Acr = np.vstack(( 100 | - pax, 101 | Aui.dot(ux) + Axi 102 | )) 103 | bcr = np.concatenate(( 104 | pa0, 105 | bi - Aui.dot(u0) 106 | )) 107 | cr = Polyhedron(Acr, bcr) 108 | cr.normalize() 109 | 110 | # optimal value function V(x) = 1/2 x' Vxx x + Vx' x + V0 111 | Vxx = ux.T.dot(self.H['uu']).dot(ux) + 2.*self.H['ux'].T.dot(ux) + self.H['xx'] 112 | Vx = (ux.T.dot(self.H['uu'].T) + self.H['ux'].T).dot(u0) + ux.T.dot(self.f['u']) + self.f['x'] 113 | V0 = .5*u0.dot(self.H['uu']).dot(u0) + self.f['u'].dot(u0) + self.g 114 | V = {'xx':Vxx, 'x':Vx, '0':V0} 115 | 116 | return CriticalRegion(active_set, u, p, V, cr) 117 | 118 | def explicit_solve_given_point(self, x, active_set_guess=None, verbose=False): 119 | """ 120 | Returns the explicit solution of the mpQP at a given point. 121 | In case a guess for the active set is provided, it first tries it. 122 | 123 | Arguments 124 | ---------- 125 | x : numpy.ndarray 126 | Point where we want to get the solution. 127 | active_set_guess : list of int 128 | Indices of the inequalities we guess are active at the given point. 129 | 130 | Reuturns 131 | ---------- 132 | instance of CriticalRegion 133 | Critical region that covers the give point (None if the given x is unfeasible). 134 | """ 135 | 136 | # first try the guess for the active set 137 | if active_set_guess is not None: 138 | cr = self.explicit_solve_given_active_set(active_set_guess) 139 | if cr is not None and cr.contains(x): 140 | return cr 141 | elif verbose: 142 | print('Wrong active-set guess:'), 143 | 144 | # otherwise solve the QP to get the active set 145 | sol = self.solve(x) 146 | if sol['active_set'] is None: 147 | if verbose: 148 | print('unfeasible sample.') 149 | return None 150 | if verbose: 151 | print('feasible sample with active set ' + str(sol['active_set']) + '.') 152 | 153 | return self.explicit_solve_given_active_set(sol['active_set']) 154 | 155 | def solve(self, x): 156 | """ 157 | Solves the QP at the given point x. 158 | 159 | Arguments 160 | ---------- 161 | x : numpy.ndarray 162 | Point where we want to get the solution. 163 | 164 | Returns 165 | ---------- 166 | sol : dict 167 | Dictionary with the solution of the QP (see the documentation of pympc.optimization.pnnls.quadratic_program for the details of the fields of sol). 168 | """ 169 | 170 | # fix cost function and constraints 171 | f = self.H['ux'].dot(x) + self.f['u'] 172 | b = self.b - self.A['x'].dot(x) 173 | sol = quadratic_program(self.H['uu'], f, self.A['u'], b) 174 | 175 | # "lift" optimal value function 176 | if sol['min'] is not None: 177 | sol['min'] += .5 * x.dot(self.H['xx']).dot(x) + self.f['x'].dot(x) + self.g 178 | 179 | return sol 180 | 181 | def explicit_solve(self, step_size=1.e-5, verbose=False): 182 | """ 183 | Returns the explicit solution of the mpQP. 184 | It assumes that the facet-to-facet property holds (i.e. each facet of a critical region is shared with another single critical region). 185 | The following is a simple home-made algorithm. 186 | For every critical region, it looks at its non-redundat inequalities and guesses the active set beyond them. 187 | It then solves the KKTs for the given active set and check if the guess was right, if not it solves a QP to get the right active set and solves the KKTs again. 188 | 189 | Arguments 190 | ---------- 191 | step_size : float 192 | Size of the step taken to explore a new critical region from the facet of its parent. 193 | verbose : bool 194 | If True it prints the number active sets found at each iteration of the solver. 195 | 196 | Returns 197 | ---------- 198 | instance of ExplicitSolution 199 | Explicit solution of the mpQP. 200 | 201 | """ 202 | 203 | # start from the origin and guess its active set 204 | x = np.zeros(self.f['x'].size) 205 | active_set_guess = [] 206 | x_buffer = [(x, active_set_guess)] 207 | crs_found = [] 208 | 209 | # loop until the are no points left 210 | while len(x_buffer) > 0: 211 | 212 | # discard points that have been already covered 213 | x_buffer = [x for x in x_buffer if not any([cr.contains(x[0]) for cr in crs_found])] 214 | if len(x_buffer) == 0: 215 | break 216 | 217 | # get critical region for the first point in the buffer 218 | cr = self.explicit_solve_given_point(x_buffer[0][0], x_buffer[0][1], verbose) 219 | del x_buffer[0] 220 | 221 | # if feasible 222 | if cr is not None: 223 | 224 | # step outside each minimal facet 225 | for i in cr.minimal_facets(): 226 | x = cr.facet_center(i) + step_size * cr.A[i] 227 | 228 | # guess the active set on the other side of the facet 229 | active_set_guess = set(cr.active_set).symmetric_difference({i}) 230 | 231 | # add to the buffer 232 | x_buffer.append((x, sorted(list(active_set_guess)))) 233 | 234 | # if feasible, add the the list of critical regions 235 | crs_found.append(cr) 236 | if verbose: 237 | print('CR found, active set: ' + str(cr.active_set) + '.') 238 | if verbose: 239 | print('Explicit solution found, CRs are: ' + str(len(crs_found)) + '.') 240 | 241 | return ExplicitSolution(crs_found) 242 | 243 | def get_feasible_set(self): 244 | """ 245 | Returns the feasible set of the mqQP, i.e. {x | exists u: Au u + Ax x <= b}. 246 | 247 | Returns 248 | ---------- 249 | instance of Polyhedron 250 | Feasible set. 251 | """ 252 | 253 | # constraint set 254 | C = Polyhedron( 255 | np.hstack((self.A['x'], self.A['u'])), 256 | self.b 257 | ) 258 | 259 | # feasible set 260 | return C.project_to(range(self.A['x'].shape[1])) 261 | 262 | class CriticalRegion(object): 263 | """ 264 | Critical Region (CR) of a multi-parametric quadratic program. 265 | A CR is the region of space where a fixed active set is optimal. 266 | """ 267 | 268 | def __init__(self, active_set, u, p, V, polyhedron): 269 | """ 270 | Instatiates the critical region. 271 | 272 | Arguments 273 | ---------- 274 | active_set : list of int 275 | List of the indices of the active inequalities. 276 | u : dict 277 | Explicit primal solution for the given active set (with keys: 'x' for the linear term, '0' for the offset term). 278 | p : dict 279 | Explicit dual solution for the given active set (with keys: 'x' for the linear term, '0' for the offset term). 280 | V : dict 281 | Explicit expression of the optimal value function for the given active set (with keys: 'xx' for the quadratic term, 'x' for the linear term, '0' for the offset term). 282 | polyhedron : instance of Polyhedron 283 | Region of space where the given active set is actually optimal. 284 | """ 285 | 286 | self.active_set = active_set 287 | self._u = u 288 | self._p = p 289 | self._V = V 290 | self.polyhedron = polyhedron 291 | 292 | def contains(self, x): 293 | """ 294 | Checks if the point x is inside the critical region. 295 | 296 | Arguments 297 | ---------- 298 | x : numpy.ndarray 299 | Point we want to check. 300 | 301 | Returns 302 | ---------- 303 | bool 304 | True if the point is contained in the critical region, False otherwise. 305 | """ 306 | return self.polyhedron.contains(x) 307 | 308 | def minimal_facets(self): 309 | """ 310 | Returns the minimal facets of the critical region. 311 | 312 | Returns 313 | ---------- 314 | list of int 315 | List of indices of the non-redundant inequalities. 316 | """ 317 | 318 | return self.polyhedron.minimal_facets() 319 | 320 | def facet_center(self, i): 321 | """ 322 | Returns the Cebyshec center of the i-th facet. 323 | Implementation note: it is necessary to add the facet as an equality constraint, otherwise if we add it as an inequality the radius of the facet is zero and the center ends up in a vertex of the facet itself, and stepping out the facet starting from vertex will not find the neighbour critical region. 324 | 325 | Arguments 326 | ---------- 327 | i : int 328 | Index of the inequality associated with the facet we want to get the center of. 329 | 330 | Returns 331 | ---------- 332 | numpy.ndarray 333 | Chebyshev center of the i-th facet. 334 | """ 335 | 336 | # handle 1-dimensional case 337 | if self.polyhedron.A.shape[1] == 1: 338 | return self.polyhedron.b.flatten()[i] / self.polyhedron.A[i][0] 339 | 340 | # add an equality to the original polyhedron 341 | facet = copy(self.polyhedron) 342 | facet.add_equality( 343 | facet.A[i:i+1, :], 344 | facet.b[i:i+1] 345 | ) 346 | 347 | return facet.center 348 | 349 | def u(self, x): 350 | """ 351 | Numeric value of the primal optimizer at the point x. 352 | 353 | Arguments 354 | ---------- 355 | x : numpy.ndarray 356 | Point where we want to get the solution. 357 | 358 | Returns 359 | ---------- 360 | numpy.ndarray 361 | Primal optimizer at the given point. 362 | """ 363 | 364 | return self._u['x'].dot(x) + self._u['0'] 365 | 366 | def p(self, x): 367 | """ 368 | Numeric value of the dual optimizer at the point x. 369 | 370 | Arguments 371 | ---------- 372 | x : numpy.ndarray 373 | Point where we want to get the solution. 374 | 375 | Returns 376 | ---------- 377 | numpy.ndarray 378 | Dual optimizer at the given point. 379 | """ 380 | 381 | return self._p['x'].dot(x) + self._p['0'] 382 | 383 | def V(self, x): 384 | """ 385 | Numeric value of the optimal value function at the point x. 386 | 387 | Arguments 388 | ---------- 389 | x : numpy.ndarray 390 | Point where we want to get the solution. 391 | 392 | Returns 393 | ---------- 394 | float 395 | Optimal value function at the given point. 396 | """ 397 | 398 | V = .5*x.dot(self._V['xx']).dot(x) + self._V['x'].dot(x) + self._V['0'] 399 | 400 | return V 401 | 402 | @property 403 | def A(self): 404 | """ 405 | Left hand side of the inequalities describing the critical region. 406 | 407 | Returns 408 | ---------- 409 | numpy.ndarray 410 | Left hand side of self.polyhedron. 411 | """ 412 | return self.polyhedron.A 413 | 414 | @property 415 | def b(self): 416 | """ 417 | Right hand side of the inequalities describing the critical region. 418 | 419 | Returns 420 | ---------- 421 | numpy.ndarray 422 | Right hand side of self.polyhedron. 423 | """ 424 | return self.polyhedron.b 425 | 426 | class ExplicitSolution(object): 427 | """ 428 | Explicit solution of a multiparametric quadratic program. 429 | """ 430 | 431 | def __init__(self, critical_regions): 432 | """ 433 | Stores the set of critical regions. 434 | 435 | Arguments 436 | ---------- 437 | critical_regions : list of intances of CriticalRegion 438 | List of crtical regions for the solution of the mpQP. 439 | """ 440 | self.critical_regions = critical_regions 441 | 442 | def get_critical_region(self, x): 443 | """ 444 | Returns the critical region that covers the given point. 445 | 446 | Arguments 447 | ---------- 448 | x : numpy.ndarray 449 | Point where we want to get the critical region. 450 | 451 | Returns 452 | ---------- 453 | instance of CriticalRegion 454 | Critical region that covers the given point (None if the point is not covered). 455 | """ 456 | 457 | # loop over the critical regions 458 | for cr in self.critical_regions: 459 | if cr.contains(x): 460 | return cr 461 | 462 | # return None if not covered 463 | return None 464 | 465 | def u(self, x): 466 | """ 467 | Numeric value of the primal optimizer at the point x. 468 | 469 | Arguments 470 | ---------- 471 | x : numpy.ndarray 472 | Point where we want to get the solution. 473 | 474 | Returns 475 | ---------- 476 | numpy.ndarray 477 | Primal optimizer at the given point (None if the point is not covered). 478 | """ 479 | 480 | # loop over the critical regions 481 | cr = self.get_critical_region(x) 482 | if cr is not None: 483 | return cr.u(x) 484 | 485 | # return None if not covered 486 | return None 487 | 488 | def p(self, x): 489 | """ 490 | Numeric value of the dual optimizer at the point x. 491 | 492 | Arguments 493 | ---------- 494 | x : numpy.ndarray 495 | Point where we want to get the solution. 496 | 497 | Returns 498 | ---------- 499 | numpy.ndarray 500 | Dual optimizer at the given point (None if the point is not covered). 501 | """ 502 | 503 | # loop over the critical regions 504 | cr = self.get_critical_region(x) 505 | if cr is not None: 506 | return cr.p(x) 507 | 508 | # return None if not covered 509 | return None 510 | 511 | def V(self, x): 512 | """ 513 | Numeric value of the optimal value function at the point x. 514 | 515 | Arguments 516 | ---------- 517 | x : numpy.ndarray 518 | Point where we want to get the solution. 519 | 520 | Returns 521 | ---------- 522 | float 523 | Optimal value function at the given point (None if the point is not covered). 524 | """ 525 | 526 | # loop over the critical regions 527 | cr = self.get_critical_region(x) 528 | if cr is not None: 529 | return cr.V(x) 530 | 531 | # return None if not covered 532 | return None 533 | 534 | class MultiParametricMixedIntegerQuadraticProgram(object): 535 | """ 536 | Multiparametric Mixed Integer Quadratic Program (mpMIQP) in the form that comes out from the MPC problem for a piecewise affine system, i.e. 537 | |u|' |Huu 0 0 0| |u| 538 | |z| | 0 Hzz 0 Hzx| |z| 539 | V(x) := min_{u,z,d} 1/2 |d| | 0 0| |d| 540 | |x| |sym Hxx| |x| 541 | s.t. Au u + Az z + Ad d + Ax x <= b 542 | where: 543 | u := (u(0), ..., u(N-1)), continuous, 544 | z := (z(0), ..., z(N-1)), continuous, 545 | d := (d(0), ..., d(N-1)), binary, 546 | while x is the intial condition. 547 | """ 548 | 549 | def __init__(self, H, A, b): 550 | """ 551 | Initializes the mpMIQP. 552 | 553 | Arguments 554 | ----------- 555 | H : dict of numpy.ndarry 556 | Dictionary with the blocks of the cost function Hessian, keys: 'uu', 'zz', 'xx', 'zx'. 557 | A : dict of numpy.ndarry 558 | Dictionary with the blocks of the constraint Jacobian, keys: 'u', 'z', 'd', 'x'. 559 | b : numpy.ndarray 560 | Right-hand side of the constraints. 561 | """ 562 | 563 | # store matrices 564 | self.H = H 565 | self.A = A 566 | self.b = b 567 | 568 | def solve(self, x): 569 | """ 570 | Solves the mpMIQP for the given value of the parameter x. 571 | 572 | Arguments 573 | ---------- 574 | x : numpy.ndarry 575 | Numeric value of the parameter vector. 576 | 577 | Returns 578 | ---------- 579 | sol : dict 580 | Dictionary with the solution of the MIQP, keys: 'min', 'u', 'z', 'd'. 581 | """ 582 | 583 | # MIQP dimensions 584 | nu = self.A['u'].shape[1] 585 | nz = self.A['z'].shape[1] 586 | nd = self.A['d'].shape[1] 587 | nc = nu + nz 588 | 589 | # put MIQP in standard form 590 | H = block_diag( 591 | self.H['uu'], 592 | self.H['zz'], 593 | np.zeros((nd, nd)) 594 | ) 595 | f = np.concatenate(( 596 | np.zeros(nu), 597 | self.H['zx'].dot(x), 598 | np.zeros(nd) 599 | )) 600 | A = np.hstack(( 601 | self.A['u'], 602 | self.A['z'], 603 | self.A['d'] 604 | )) 605 | b = self.b - self.A['x'].dot(x) 606 | 607 | # solve MIQP 608 | sol_sf = mixed_integer_quadratic_program(nc, H, f, A, b) 609 | 610 | # reshape solution 611 | sol = { 612 | 'min': sol_sf['min'], 613 | 'u': None, 614 | 'z': None, 615 | 'd': None 616 | } 617 | 618 | # if feasible lift the cost function with the offset term 619 | if sol['min'] is not None: 620 | sol['min'] += .5*x.dot(self.H['xx']).dot(x) 621 | sol['u'] = sol_sf['argmin'][:nu] 622 | sol['z'] = sol_sf['argmin'][nu:nu+nz] 623 | sol['d'] = sol_sf['argmin'][nu+nz:] 624 | 625 | return sol -------------------------------------------------------------------------------- /pympc/optimization/programs.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import numpy as np 3 | 4 | # internal inputs 5 | from pympc.optimization.solvers.pnnls import linear_program as lp_pnnls, quadratic_program as qp_pnnls 6 | from pympc.optimization.solvers.gurobi import linear_program as lp_gurobi, quadratic_program as qp_gurobi, mixed_integer_quadratic_program as miqp_gurobi 7 | # from pympc.optimization.solvers.drake import linear_program as lp_drake, quadratic_program as qp_drake, mixed_integer_quadratic_program as miqp_drake 8 | 9 | def linear_program(f, A, b, C=None, d=None, solver='pnnls'): 10 | """ 11 | Calls the desired solver to solve the linear program min_x f^T x s.t. A x <= b, C x = d. 12 | 13 | Arguments 14 | ---------- 15 | f : numpy.ndarray 16 | Gradient of the cost function. 17 | A : numpy.ndarray 18 | Left-hand side of the inequality constraints. 19 | b : numpy.ndarray 20 | Right-hand side of the inequality constraints. 21 | C : numpy.ndarray 22 | Left-hand side of the equality constraints. 23 | d : numpy.ndarray 24 | Right-hand side of the equality constraints. 25 | solver : str 26 | Name of the solver to be used, available solvers are: 'pnnls', 'gurobi', 'drake'. 27 | 28 | Returns 29 | ---------- 30 | sol : dict 31 | Dictionary with the solution of the LP. 32 | 33 | Keys 34 | ---------- 35 | min : float 36 | Minimum of the LP (None if the problem is unfeasible or unbounded). 37 | argmin : numpy.ndarray 38 | Argument that minimizes the LP (None if the problem is unfeasible or unbounded). 39 | active_set : list of int 40 | Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible or unbounded). 41 | multiplier_inequality : numpy.ndarray 42 | Lagrange multipliers for the inequality constraints (None if the problem is unfeasible or unbounded). 43 | multiplier_equality : numpy.ndarray 44 | Lagrange multipliers for the equality constraints (None if the problem is unfeasible or unbounded or without equality constraints). 45 | """ 46 | 47 | # select solver 48 | if solver == 'pnnls': 49 | return lp_pnnls(f, A, b, C, d) 50 | elif solver == 'gurobi': 51 | return lp_gurobi(f, A, b, C, d) 52 | elif solver == 'drake': 53 | return lp_drake(f, A, b, C, d) 54 | else: 55 | raise ValueError('unknown solver ' + str(solver) + '.') 56 | 57 | def quadratic_program(H, f, A, b, C=None, d=None, solver='pnnls'): 58 | """ 59 | Calls the desired solver to solve the strictly convex (H > 0) quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d. 60 | 61 | Arguments 62 | ---------- 63 | H : numpy.ndarray 64 | Positive definite Hessian of the cost function. 65 | f : numpy.ndarray 66 | Gradient of the cost function. 67 | A : numpy.ndarray 68 | Left-hand side of the inequality constraints. 69 | b : numpy.ndarray 70 | Right-hand side of the inequality constraints. 71 | C : numpy.ndarray 72 | Left-hand side of the equality constraints. 73 | d : numpy.ndarray 74 | Right-hand side of the equality constraints. 75 | solver : str 76 | Name of the solver to be used, available solvers are: 'pnnls', 'gurobi', 'drake'. 77 | 78 | Returns 79 | ---------- 80 | sol : dict 81 | Dictionary with the solution of the QP. 82 | 83 | Fields 84 | ---------- 85 | min : float 86 | Minimum of the QP (None if the problem is unfeasible). 87 | argmin : numpy.ndarray 88 | Argument that minimizes the QP (None if the problem is unfeasible). 89 | active_set : list of int 90 | Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible). 91 | multiplier_inequality : numpy.ndarray 92 | Lagrange multipliers for the inequality constraints (None if the problem is unfeasible). 93 | multiplier_equality : numpy.ndarray 94 | Lagrange multipliers for the equality constraints (None if the problem is unfeasible or without equality constraints). 95 | """ 96 | 97 | # select solver 98 | if solver == 'pnnls': 99 | return qp_pnnls(H, f, A, b, C, d) 100 | elif solver == 'gurobi': 101 | return qp_gurobi(H, f, A, b, C, d) 102 | elif solver == 'drake': 103 | return qp_drake(H, f, A, b, C, d) 104 | else: 105 | raise ValueError('unknown solver ' + str(solver) + '.') 106 | 107 | def mixed_integer_quadratic_program(nc, H, f, A, b, C=None, d=None, solver='gurobi'): 108 | """ 109 | Calls the desired solver to solve the strictly convex (H > 0) mixed-integer quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d. 110 | The first nc variables in x are continuous, the remaining are binaries. 111 | 112 | Arguments 113 | ---------- 114 | nc : int 115 | Number of continuous variables in the problem. 116 | H : numpy.ndarray 117 | Positive definite Hessian of the cost function. 118 | f : numpy.ndarray 119 | Gradient of the cost function. 120 | A : numpy.ndarray 121 | Left-hand side of the inequality constraints. 122 | b : numpy.ndarray 123 | Right-hand side of the inequality constraints. 124 | C : numpy.ndarray 125 | Left-hand side of the equality constraints. 126 | d : numpy.ndarray 127 | Right-hand side of the equality constraints. 128 | solver : str 129 | Name of the solver to be used, available solvers are: 'pnnls', 'gurobi', 'drake'. 130 | 131 | Returns 132 | ---------- 133 | sol : dict 134 | Dictionary with the solution of the MIQP. 135 | 136 | Fields 137 | ---------- 138 | min : float 139 | Minimum of the MIQP (None if the problem is unfeasible). 140 | argmin : numpy.ndarray 141 | Argument that minimizes the MIQP (None if the problem is unfeasible). 142 | """ 143 | 144 | # select solver 145 | if solver == 'gurobi': 146 | return miqp_gurobi(nc, H, f, A, b, C, d) 147 | if solver == 'drake': 148 | return miqp_drake(nc, H, f, A, b, C, d) 149 | else: 150 | raise ValueError('unknown solver ' + str(solver) + '.') -------------------------------------------------------------------------------- /pympc/optimization/solvers/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/pympc/optimization/solvers/__init__.py -------------------------------------------------------------------------------- /pympc/optimization/solvers/drake.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import numpy as np 3 | from pydrake.all import MathematicalProgram, SolutionResult 4 | from pydrake.solvers.gurobi import GurobiSolver 5 | 6 | def linear_program(f, A, b, C=None, d=None, tol=1.e-5, **kwargs): 7 | """ 8 | Solves the linear program min_x f^T x s.t. A x <= b, C x = d. 9 | 10 | Arguments 11 | ---------- 12 | f : numpy.ndarray 13 | Gradient of the cost function. 14 | A : numpy.ndarray 15 | Left-hand side of the inequality constraints. 16 | b : numpy.ndarray 17 | Right-hand side of the inequality constraints. 18 | C : numpy.ndarray 19 | Left-hand side of the equality constraints. 20 | d : numpy.ndarray 21 | Right-hand side of the equality constraints. 22 | tol : float 23 | Maximum value of a residual of an inequality to consider the related constraint active. 24 | 25 | Returns 26 | ---------- 27 | sol : dict 28 | Dictionary with the solution of the LP. 29 | 30 | Fields 31 | ---------- 32 | min : float 33 | Minimum of the LP (None if the problem is unfeasible or unbounded). 34 | argmin : numpy.ndarray 35 | Argument that minimizes the LP (None if the problem is unfeasible or unbounded). 36 | active_set : list of int 37 | Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible or unbounded). 38 | multiplier_inequality : numpy.ndarray 39 | Lagrange multipliers for the inequality constraints (None if the problem is unfeasible or unbounded). 40 | multiplier_equality : numpy.ndarray 41 | Lagrange multipliers for the equality constraints (None if the problem is unfeasible or unbounded or without equality constraints). 42 | """ 43 | 44 | # check equalities 45 | if (C is None) != (d is None): 46 | raise ValueError('missing C or d.') 47 | 48 | # problem size 49 | n_ineq, n_x = A.shape 50 | if C is not None: 51 | n_eq = C.shape[0] 52 | else: 53 | n_eq = 0 54 | 55 | # build program 56 | prog = MathematicalProgram() 57 | x = prog.NewContinuousVariables(n_x) 58 | [prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)] 59 | [prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)] 60 | prog.AddLinearCost(f.dot(x)) 61 | 62 | # solve 63 | solver = GurobiSolver() 64 | prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0) 65 | [prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()] 66 | result = prog.Solve() 67 | 68 | # initialize output 69 | sol = { 70 | 'min': None, 71 | 'argmin': None, 72 | 'active_set': None, 73 | 'multiplier_inequality': None, 74 | 'multiplier_equality': None 75 | } 76 | 77 | if result == SolutionResult.kSolutionFound: 78 | sol['argmin'] = prog.GetSolution(x) 79 | sol['min'] = f.dot(sol['argmin']) 80 | sol['active_set'] = np.where(A.dot(sol['argmin']) - b > -tol)[0].tolist() 81 | 82 | # retrieve multipliers through KKT conditions 83 | M = A[sol['active_set']].T 84 | if n_eq > 0: 85 | M = np.hstack((M, C.T)) 86 | m = -np.linalg.pinv(M).dot(f) 87 | sol['multiplier_inequality'] = np.zeros(n_ineq) 88 | for i, j in enumerate(sol['active_set']): 89 | sol['multiplier_inequality'][j] = m[i] 90 | if n_eq > 0: 91 | sol['multiplier_equality'] = m[len(sol['active_set']):] 92 | 93 | return sol 94 | 95 | def quadratic_program(H, f, A, b, C=None, d=None, tol=1.e-5, **kwargs): 96 | """ 97 | Solves the strictly convex (H > 0) quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d. 98 | 99 | Arguments 100 | ---------- 101 | H : numpy.ndarray 102 | Positive definite Hessian of the cost function. 103 | f : numpy.ndarray 104 | Gradient of the cost function. 105 | A : numpy.ndarray 106 | Left-hand side of the inequality constraints. 107 | b : numpy.ndarray 108 | Right-hand side of the inequality constraints. 109 | C : numpy.ndarray 110 | Left-hand side of the equality constraints. 111 | d : numpy.ndarray 112 | Right-hand side of the equality constraints. 113 | tol : float 114 | Maximum value of a residual of an inequality to consider the related constraint active. 115 | 116 | Returns 117 | ---------- 118 | sol : dict 119 | Dictionary with the solution of the QP. 120 | 121 | Fields 122 | ---------- 123 | min : float 124 | Minimum of the QP (None if the problem is unfeasible). 125 | argmin : numpy.ndarray 126 | Argument that minimizes the QP (None if the problem is unfeasible). 127 | active_set : list of int 128 | Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible). 129 | multiplier_inequality : numpy.ndarray 130 | Lagrange multipliers for the inequality constraints (None if the problem is unfeasible). 131 | multiplier_equality : numpy.ndarray 132 | Lagrange multipliers for the equality constraints (None if the problem is unfeasible or without equality constraints). 133 | """ 134 | 135 | # check equalities 136 | if (C is None) != (d is None): 137 | raise ValueError('missing C or d.') 138 | 139 | # problem size 140 | n_ineq, n_x = A.shape 141 | if C is not None: 142 | n_eq = C.shape[0] 143 | else: 144 | n_eq = 0 145 | 146 | # build program 147 | prog = MathematicalProgram() 148 | x = prog.NewContinuousVariables(n_x) 149 | [prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)] 150 | [prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)] 151 | inequalities = [] 152 | prog.AddQuadraticCost(.5*x.dot(H).dot(x) + f.dot(x)) 153 | 154 | # solve 155 | solver = GurobiSolver() 156 | prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0) 157 | [prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()] 158 | result = prog.Solve() 159 | 160 | # initialize output 161 | sol = { 162 | 'min': None, 163 | 'argmin': None, 164 | 'active_set': None, 165 | 'multiplier_inequality': None, 166 | 'multiplier_equality': None 167 | } 168 | 169 | if result == SolutionResult.kSolutionFound: 170 | sol['argmin'] = prog.GetSolution(x) 171 | sol['min'] = .5*sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin']) 172 | sol['active_set'] = np.where(A.dot(sol['argmin']) - b > -tol)[0].tolist() 173 | 174 | # retrieve multipliers through KKT conditions 175 | lhs = A[sol['active_set']] 176 | rhs = b[sol['active_set']] 177 | if n_eq > 0: 178 | lhs = np.vstack((lhs, C)) 179 | rhs = np.concatenate((rhs, d)) 180 | H_inv = np.linalg.inv(H) 181 | M = lhs.dot(H_inv).dot(lhs.T) 182 | m = - np.linalg.inv(M).dot(lhs.dot(H_inv).dot(f) + rhs) 183 | sol['multiplier_inequality'] = np.zeros(n_ineq) 184 | for i, j in enumerate(sol['active_set']): 185 | sol['multiplier_inequality'][j] = m[i] 186 | if n_eq > 0: 187 | sol['multiplier_equality'] = m[len(sol['active_set']):] 188 | 189 | return sol 190 | 191 | def mixed_integer_quadratic_program(nc, H, f, A, b, C=None, d=None, **kwargs): 192 | """ 193 | Solves the strictly convex (H > 0) mixed-integer quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d. 194 | The first nc variables in x are continuous, the remaining are binaries. 195 | 196 | Arguments 197 | ---------- 198 | nc : int 199 | Number of continuous variables in the problem. 200 | H : numpy.ndarray 201 | Positive definite Hessian of the cost function. 202 | f : numpy.ndarray 203 | Gradient of the cost function. 204 | A : numpy.ndarray 205 | Left-hand side of the inequality constraints. 206 | b : numpy.ndarray 207 | Right-hand side of the inequality constraints. 208 | C : numpy.ndarray 209 | Left-hand side of the equality constraints. 210 | d : numpy.ndarray 211 | Right-hand side of the equality constraints. 212 | 213 | Returns 214 | ---------- 215 | sol : dict 216 | Dictionary with the solution of the MIQP. 217 | 218 | Fields 219 | ---------- 220 | min : float 221 | Minimum of the MIQP (None if the problem is unfeasible). 222 | argmin : numpy.ndarray 223 | Argument that minimizes the MIQP (None if the problem is unfeasible). 224 | """ 225 | 226 | # check equalities 227 | if (C is None) != (d is None): 228 | raise ValueError('missing C or d.') 229 | 230 | # problem size 231 | n_ineq, n_x = A.shape 232 | if C is not None: 233 | n_eq = C.shape[0] 234 | else: 235 | n_eq = 0 236 | 237 | # build program 238 | prog = MathematicalProgram() 239 | x = np.hstack(( 240 | prog.NewContinuousVariables(nc), 241 | prog.NewBinaryVariables(n_x - nc) 242 | )) 243 | [prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)] 244 | [prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)] 245 | prog.AddQuadraticCost(.5*x.dot(H).dot(x) + f.dot(x)) 246 | 247 | # solve 248 | solver = GurobiSolver() 249 | prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0) 250 | [prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()] 251 | result = prog.Solve() 252 | 253 | # initialize output 254 | sol = { 255 | 'min': None, 256 | 'argmin': None 257 | } 258 | 259 | if result == SolutionResult.kSolutionFound: 260 | sol['argmin'] = prog.GetSolution(x) 261 | sol['min'] = .5*sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin']) 262 | 263 | return sol -------------------------------------------------------------------------------- /pympc/optimization/solvers/gurobi.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import numpy as np 3 | import gurobipy as grb 4 | 5 | def linear_program(f, A, b, C=None, d=None, **kwargs): 6 | """ 7 | Solves the linear program min_x f^T x s.t. A x <= b, C x = d. 8 | 9 | Arguments 10 | ---------- 11 | f : numpy.ndarray 12 | Gradient of the cost function. 13 | A : numpy.ndarray 14 | Left-hand side of the inequality constraints. 15 | b : numpy.ndarray 16 | Right-hand side of the inequality constraints. 17 | C : numpy.ndarray 18 | Left-hand side of the equality constraints. 19 | d : numpy.ndarray 20 | Right-hand side of the equality constraints. 21 | 22 | Returns 23 | ---------- 24 | sol : dict 25 | Dictionary with the solution of the LP. 26 | 27 | Fields 28 | ---------- 29 | min : float 30 | Minimum of the LP (None if the problem is unfeasible or unbounded). 31 | argmin : numpy.ndarray 32 | Argument that minimizes the LP (None if the problem is unfeasible or unbounded). 33 | active_set : list of int 34 | Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible or unbounded). 35 | multiplier_inequality : numpy.ndarray 36 | Lagrange multipliers for the inequality constraints (None if the problem is unfeasible or unbounded). 37 | multiplier_equality : numpy.ndarray 38 | Lagrange multipliers for the equality constraints (None if the problem is unfeasible or unbounded or without equality constraints). 39 | """ 40 | 41 | # get model 42 | model = _build_model(f=f, A=A, b=b, C=C, d=d) 43 | 44 | # set the parameters 45 | model.setParam('OutputFlag', 0) 46 | [model.setParam(parameter, value) for parameter, value in kwargs.items()] 47 | 48 | # run the optimization 49 | model.optimize() 50 | 51 | # return result 52 | sol = _reorganize_solution(model, A, C) 53 | 54 | # get active set 55 | if model.status == grb.GRB.Status.OPTIMAL: 56 | sol['active_set'] = [i for i in range(A.shape[0]) if model.getConstrByName('ineq_'+str(i)).getAttr('CBasis') == -1] 57 | 58 | return sol 59 | 60 | def quadratic_program(H, f, A, b, C=None, d=None, tol=1.e-5, **kwargs): 61 | """ 62 | Solves the strictly convex (H > 0) quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d. 63 | 64 | Arguments 65 | ---------- 66 | H : numpy.ndarray 67 | Positive definite Hessian of the cost function. 68 | f : numpy.ndarray 69 | Gradient of the cost function. 70 | A : numpy.ndarray 71 | Left-hand side of the inequality constraints. 72 | b : numpy.ndarray 73 | Right-hand side of the inequality constraints. 74 | C : numpy.ndarray 75 | Left-hand side of the equality constraints. 76 | d : numpy.ndarray 77 | Right-hand side of the equality constraints. 78 | tol : float 79 | Maximum value of a multiplier to consider the related constraint inactive. 80 | 81 | Returns 82 | ---------- 83 | sol : dict 84 | Dictionary with the solution of the QP. 85 | 86 | Fields 87 | ---------- 88 | min : float 89 | Minimum of the QP (None if the problem is unfeasible). 90 | argmin : numpy.ndarray 91 | Argument that minimizes the QP (None if the problem is unfeasible). 92 | active_set : list of int 93 | Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible). 94 | multiplier_inequality : numpy.ndarray 95 | Lagrange multipliers for the inequality constraints (None if the problem is unfeasible). 96 | multiplier_equality : numpy.ndarray 97 | Lagrange multipliers for the equality constraints (None if the problem is unfeasible or without equality constraints). 98 | """ 99 | 100 | # get model 101 | model = _build_model(H=H, f=f, A=A, b=b, C=C, d=d) 102 | 103 | # parameters 104 | model.setParam('OutputFlag', 0) 105 | model.setParam('BarConvTol', 1.e-10) # with the default value (1e-8) inactive multipliers can get values such as 5e-4, setting this to 1e-10 they generally are lower than 2e-6 (note that in the following the active set is retrieved looking at the numeric values of the multipliers!) 106 | [model.setParam(parameter, value) for parameter, value in kwargs.items()] 107 | 108 | # run the optimization 109 | model.optimize() 110 | 111 | # return result 112 | sol = _reorganize_solution(model, A, C) 113 | 114 | # compute active set 115 | if model.status == grb.GRB.Status.OPTIMAL: 116 | sol['active_set'] = np.where(sol['multiplier_inequality'] > tol)[0].tolist() 117 | 118 | return sol 119 | 120 | def mixed_integer_quadratic_program(nc, H, f, A, b, C=None, d=None, **kwargs): 121 | """ 122 | Solves the strictly convex (H > 0) mixed-integer quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d. 123 | The first nc variables in x are continuous, the remaining are binaries. 124 | 125 | Arguments 126 | ---------- 127 | nc : int 128 | Number of continuous variables in the problem. 129 | H : numpy.ndarray 130 | Positive definite Hessian of the cost function. 131 | f : numpy.ndarray 132 | Gradient of the cost function. 133 | A : numpy.ndarray 134 | Left-hand side of the inequality constraints. 135 | b : numpy.ndarray 136 | Right-hand side of the inequality constraints. 137 | C : numpy.ndarray 138 | Left-hand side of the equality constraints. 139 | d : numpy.ndarray 140 | Right-hand side of the equality constraints. 141 | 142 | Returns 143 | ---------- 144 | sol : dict 145 | Dictionary with the solution of the MIQP. 146 | 147 | Fields 148 | ---------- 149 | min : float 150 | Minimum of the MIQP (None if the problem is unfeasible). 151 | argmin : numpy.ndarray 152 | Argument that minimizes the MIQP (None if the problem is unfeasible). 153 | """ 154 | 155 | # initialize model 156 | model = _build_model(H=H, f=f, A=A, b=b, C=C, d=d) 157 | model.update() 158 | [xi.setAttr('vtype', grb.GRB.BINARY) for xi in model.getVars()[nc:]] 159 | model.update() 160 | 161 | # parameters 162 | model.setParam('OutputFlag', 0) 163 | [model.setParam(parameter, value) for parameter, value in kwargs.items()] 164 | 165 | # run the optimization 166 | model.optimize() 167 | 168 | # return result 169 | sol = _reorganize_solution(model, A, C, continuous=False) 170 | 171 | return sol 172 | 173 | def _build_model(H=None, f=None, A=None, b=None, C=None, d=None): 174 | """ 175 | Builds the Gurobi model the LP or the QP. 176 | 177 | Arguments 178 | ---------- 179 | H, f, A, b, C, d : numpy.ndarray 180 | Matrices of the mathematical program. 181 | 182 | Returns 183 | ---------- 184 | model : instance of gurobipy.Model 185 | Model of the mathematical program. 186 | """ 187 | 188 | # initialize model 189 | model = grb.Model() 190 | n_x = f.size 191 | x = model.addVars(n_x, lb=[-grb.GRB.INFINITY]*n_x) 192 | 193 | # linear inequalities 194 | for i, expr in enumerate(linear_expression(A, -b, x)): 195 | model.addConstr(expr <= 0., name='ineq_'+str(i)) 196 | 197 | # linear equalities 198 | if C is not None and d is not None: 199 | for i, expr in enumerate(linear_expression(C, -d, x)): 200 | model.addConstr(expr == 0., name='eq_'+str(i)) 201 | 202 | # cost function 203 | if H is not None: 204 | cost = grb.QuadExpr() 205 | expr = quadratic_expression(H, x) 206 | cost.add(.5 * expr) 207 | else: 208 | cost = grb.LinExpr() 209 | f = f.reshape(1, f.size) 210 | expr = linear_expression(f, np.zeros(1), x) 211 | cost.add(expr[0]) 212 | model.setObjective(cost) 213 | 214 | return model 215 | 216 | def _reorganize_solution(model, A, C, continuous=True): 217 | """ 218 | Organizes the solution in a dictionary. 219 | 220 | Arguments 221 | ---------- 222 | model : instance of gurobipy.Model 223 | Model of the mathematical program. 224 | A : numpy.ndarray 225 | Left-hand side of the inequality constraints. 226 | C : numpy.ndarray 227 | Left-hand side of the equality constraints. 228 | continuous : bool 229 | True if the program does not contain integer variables, False otherwise. 230 | 231 | Returns 232 | ---------- 233 | sol : dict 234 | Dictionary with the solution of the mathematical program. 235 | """ 236 | 237 | # intialize solution 238 | sol = {'min': None,'argmin': None} 239 | if continuous: 240 | sol['active_set'] = None 241 | sol['multiplier_inequality'] = None 242 | sol['multiplier_equality'] = None 243 | 244 | # if optimal solution found 245 | if model.status == grb.GRB.Status.OPTIMAL: 246 | 247 | # primal solution 248 | x = model.getVars() 249 | sol['min'] = model.objVal 250 | sol['argmin'] = np.array(model.getAttr('x')) 251 | 252 | # dual solution 253 | if continuous: 254 | sol['multiplier_inequality'] = np.array([-model.getConstrByName('ineq_'+str(i)).getAttr('Pi') for i in range(A.shape[0])]) 255 | if C is not None and C.shape[0] > 0: 256 | sol['multiplier_equality'] = np.array([-model.getConstrByName('eq_'+str(i)).getAttr('Pi') for i in range(C.shape[0])]) 257 | 258 | return sol 259 | 260 | def linear_expression(A, b, x, tol=1.e-9): 261 | """ 262 | Generates a list of Gurobi linear expressions A_i x + b_i (one element per row of A). 263 | 264 | Arguments 265 | ---------- 266 | A : numpy.ndarray 267 | Linear term. 268 | b : numpy.ndarray 269 | Offest term. 270 | x : instance of gurobipy.Var 271 | Variable of the linear expression. 272 | tol : float 273 | Maximum absolute value for the elements of A and b to be considered nonzero. 274 | 275 | Returns 276 | ---------- 277 | exprs : list of gurobipy.LinExpr 278 | List of linear expressions. 279 | """ 280 | 281 | # linear term (explicitly state that it is a LinExpr since it can be that A[i] = 0) 282 | exprs = [grb.LinExpr(sum([A[i,j]*x[j] for j in range(A.shape[1]) if np.abs(A[i,j]) > tol])) for i in range(A.shape[0])] 283 | 284 | # offset term 285 | exprs = [expr+b[i] if np.abs(b[i]) > tol else expr for i, expr in enumerate(exprs)] 286 | 287 | return exprs 288 | 289 | def quadratic_expression(H, x, tol=1.e-9): 290 | """ 291 | Generates a Gurobi quadratic expressions x' H x. 292 | 293 | Arguments 294 | ---------- 295 | H : numpy.ndarray 296 | Hessian of the quadratic expression. 297 | x : instance of gurobipy.Var 298 | Variable of the linear expression. 299 | tol : float 300 | Maximum absolute value for the elements of H to be considered nonzero. 301 | 302 | Returns 303 | ---------- 304 | expr : gurobipy.LinExpr 305 | Quadratic expressions. 306 | """ 307 | 308 | return sum([x[i]*H[i,j]*x[j] for i, j in np.ndindex(H.shape) if np.abs(H[i,j]) > tol]) -------------------------------------------------------------------------------- /pympc/optimization/solvers/pnnls.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import numpy as np 3 | from scipy.optimize import nnls 4 | 5 | def pnnls(A, B, c): 6 | """ 7 | Solves the Partial Non-Negative Least Squares problem min_{u, v} ||A v + B u - c||_2^2 s.t. v >= 0. 8 | (See "Bemporad - A Multiparametric Quadratic Programming Algorithm With Polyhedral Computations Based on Nonnegative Least Squares", Lemma 1.) 9 | 10 | Arguments 11 | ---------- 12 | A : numpy.ndarray 13 | Coefficient matrix of nonnegative variables. 14 | B : numpy.ndarray 15 | Coefficient matrix of remaining variables. 16 | c : numpy.ndarray 17 | Offset term. 18 | 19 | Returns 20 | ---------- 21 | v : numpy.ndarray 22 | Optimal value of v. 23 | u : numpy.ndarray 24 | Optimal value of u. 25 | r : numpy.ndarray 26 | Residuals of the least squares problem. 27 | """ 28 | 29 | # matrices for nnls solver 30 | B_pinv = np.linalg.pinv(B) 31 | B_bar = np.eye(A.shape[0]) - B.dot(B_pinv) 32 | A_bar = B_bar.dot(A) 33 | b_bar = B_bar.dot(c) 34 | 35 | # solve nnls 36 | v, r = nnls(A_bar, b_bar) 37 | u = - B_pinv.dot(A.dot(v) - c) 38 | 39 | return v, u, r 40 | 41 | def linear_program(f, A, b, C=None, d=None, tol=1.e-7): 42 | """ 43 | Solves the linear program min_x f^T x s.t. A x <= b, C x = d. 44 | Finds a partially nonnegative least squares solution to the KKT conditions of the LP. 45 | 46 | Math 47 | ---------- 48 | For the LP min_x f^T x s.t. A x <= b, we can substitute the complementarity condition with the condition of zero duality gap, to get the linear system: 49 | b' y + f' x = 0, zero duality gap, 50 | A x + b = - s, s >= 0, primal feasibility, 51 | A' y = f, y >= 0, dual feasibility, 52 | where y are the Lagrange multipliers and s are slack variables for the residual of primal feasibility. 53 | (Each equality constraint is reformulated as two inequalities.) 54 | 55 | Arguments 56 | ---------- 57 | f : numpy.ndarray 58 | Gradient of the cost function. 59 | A : numpy.ndarray 60 | Left-hand side of the inequality constraints. 61 | b : numpy.ndarray 62 | Right-hand side of the inequality constraints. 63 | C : numpy.ndarray 64 | Left-hand side of the equality constraints. 65 | d : numpy.ndarray 66 | Right-hand side of the equality constraints. 67 | tol : float 68 | Maximum value for: the residual of the pnnls to consider the problem feasible, for the residual of the inequalities to be considered active. 69 | 70 | Returns 71 | ---------- 72 | sol : dict 73 | Dictionary with the solution of the LP. 74 | 75 | Keys 76 | ---------- 77 | min : float 78 | Minimum of the LP (None if the problem is unfeasible or unbounded). 79 | argmin : numpy.ndarray 80 | Argument that minimizes the LP (None if the problem is unfeasible or unbounded). 81 | active_set : list of int 82 | Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible or unbounded). 83 | multiplier_inequality : numpy.ndarray 84 | Lagrange multipliers for the inequality constraints (None if the problem is unfeasible or unbounded). 85 | multiplier_equality : numpy.ndarray 86 | Lagrange multipliers for the equality constraints (None if the problem is unfeasible or unbounded or without equality constraints). 87 | """ 88 | 89 | # check equalities 90 | if (C is None) != (d is None): 91 | raise ValueError('missing C or d.') 92 | 93 | # problem size 94 | n_ineq, n_x = A.shape 95 | if C is not None: 96 | n_eq = C.shape[0] 97 | else: 98 | n_eq = 0 99 | 100 | # state equalities as inequalities 101 | if n_eq > 0: 102 | AC = np.vstack((A, C, -C)) 103 | bd = np.concatenate((b, d, -d)) 104 | else: 105 | AC = A 106 | bd = b 107 | 108 | # build and solve pnnls problem 109 | A_pnnls = np.vstack(( 110 | np.concatenate(( 111 | bd, 112 | np.zeros(n_ineq + 2*n_eq) 113 | )), 114 | np.hstack(( 115 | np.zeros((n_ineq + 2*n_eq, n_ineq + 2*n_eq)), 116 | np.eye(n_ineq + 2*n_eq) 117 | )), 118 | np.hstack(( 119 | AC.T, 120 | np.zeros((n_x, n_ineq + 2*n_eq)) 121 | )) 122 | )) 123 | B_pnnls = np.vstack((f, AC, np.zeros((n_x, n_x)))) 124 | c_pnnls = np.concatenate((np.zeros(1), bd, -f)) 125 | ys, x, r = pnnls(A_pnnls, B_pnnls, c_pnnls) 126 | 127 | # initialize output 128 | sol = { 129 | 'min': None, 130 | 'argmin': None, 131 | 'active_set': None, 132 | 'multiplier_inequality': None, 133 | 'multiplier_equality': None 134 | } 135 | 136 | # fill solution if residual is almost zero 137 | if r < tol: 138 | sol['argmin'] = x 139 | sol['min'] = f.dot(sol['argmin']) 140 | sol['multiplier_inequality'] = ys[:n_ineq] 141 | sol['active_set'] = sorted(np.where(sol['multiplier_inequality'] > tol)[0]) 142 | if n_eq > 0: 143 | mul_eq_pos = ys[n_ineq:n_ineq+n_eq] 144 | mul_eq_neg = - ys[n_ineq+n_eq:n_ineq+2*n_eq] 145 | sol['multiplier_equality'] = mul_eq_pos + mul_eq_neg 146 | 147 | return sol 148 | 149 | def quadratic_program(H, f, A, b, C=None, d=None, tol=1.e-7): 150 | """ 151 | Solves the strictly convex (H > 0) quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d using nonnegative least squres. 152 | (See "Bemporad - A Quadratic Programming Algorithm Based on Nonnegative Least Squares With Applications to Embedded Model Predictive Control", Theorem 1.) 153 | 154 | Arguments 155 | ---------- 156 | H : numpy.ndarray 157 | Positive definite Hessian of the cost function. 158 | f : numpy.ndarray 159 | Gradient of the cost function. 160 | A : numpy.ndarray 161 | Left-hand side of the inequality constraints. 162 | b : numpy.ndarray 163 | Right-hand side of the inequality constraints. 164 | C : numpy.ndarray 165 | Left-hand side of the equality constraints. 166 | d : numpy.ndarray 167 | Right-hand side of the equality constraints. 168 | tol : float 169 | Maximum value for: the residual of the pnnls to consider the problem unfeasible, for the residual of an inequality to consider the constraint active. 170 | 171 | Returns 172 | ---------- 173 | sol : dict 174 | Dictionary with the solution of the QP. 175 | 176 | Fields 177 | ---------- 178 | min : float 179 | Minimum of the QP (None if the problem is unfeasible). 180 | argmin : numpy.ndarray 181 | Argument that minimizes the QP (None if the problem is unfeasible). 182 | active_set : list of int 183 | Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible). 184 | multiplier_inequality : numpy.ndarray 185 | Lagrange multipliers for the inequality constraints (None if the problem is unfeasible). 186 | multiplier_equality : numpy.ndarray 187 | Lagrange multipliers for the equality constraints (None if the problem is unfeasible or without equality constraints). 188 | """ 189 | 190 | # check equalities 191 | if (C is None) != (d is None): 192 | raise ValueError('missing C or d.') 193 | 194 | # problem size 195 | n_ineq, n_x = A.shape 196 | if C is not None: 197 | n_eq = C.shape[0] 198 | else: 199 | n_eq = 0 200 | 201 | # state equalities as inequalities 202 | if n_eq > 0: 203 | AC = np.vstack((A, C, -C)) 204 | bd = np.concatenate((b, d, -d)) 205 | else: 206 | AC = A 207 | bd = b 208 | 209 | # build and solve pnnls problem 210 | L = np.linalg.cholesky(H) 211 | L_inv = np.linalg.inv(L) 212 | H_inv = L_inv.T.dot(L_inv) 213 | M = AC.dot(L_inv.T) 214 | m = bd + AC.dot(H_inv).dot(f) 215 | gamma = np.ones(1) 216 | A_nnls = np.vstack((- M.T, - m)) 217 | b_nnls = np.concatenate((np.zeros(n_x), gamma)) 218 | y, r = nnls(A_nnls, b_nnls) 219 | 220 | # initialize output 221 | sol = { 222 | 'min': None, 223 | 'argmin': None, 224 | 'active_set': None, 225 | 'multiplier_inequality': None, 226 | 'multiplier_equality': None 227 | } 228 | 229 | # if feasibile 230 | if r > tol: 231 | lam = y / (gamma[0] + m.dot(y)) 232 | sol['multiplier_inequality'] = lam[:n_ineq] 233 | sol['argmin'] = - H_inv.dot(f + AC.T.dot(lam)) 234 | sol['min'] = .5 * sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin']) 235 | sol['active_set'] = sorted(np.where(sol['multiplier_inequality'] > tol)[0]) 236 | if n_eq > 0: 237 | mul_eq_pos = lam[n_ineq:n_ineq+n_eq] 238 | mul_eq_neg = - lam[n_ineq+n_eq:n_ineq+2*n_eq] 239 | sol['multiplier_equality'] = mul_eq_pos + mul_eq_neg 240 | 241 | return sol -------------------------------------------------------------------------------- /pympc/plot.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | 5 | def plot_state_space_trajectory(x, dim=[0,1], text=False, label=None, **kwargs): 6 | """ 7 | Plots one component of the state x as a function of another (2d plot). 8 | 9 | Arguments 10 | ---------- 11 | x : list of numpy.ndarray 12 | Trajectory of the state. 13 | dim : list of int 14 | List of the indices of the components of the state that we want to plot (2 indices). 15 | """ 16 | 17 | # check inputs 18 | if len(dim) != 2: 19 | raise ValueError('can plot only 2-dimensional trajectories.') 20 | 21 | # plot trajectory 22 | for t in range(len(x)-1): 23 | if t == 0: 24 | plt.plot( 25 | [x[0][dim[0]], x[1][dim[0]]], 26 | [x[0][dim[1]], x[1][dim[1]]], 27 | label=label, 28 | **kwargs 29 | ) 30 | plt.plot( 31 | [x[t][dim[0]], x[t+1][dim[0]]], 32 | [x[t][dim[1]], x[t+1][dim[1]]], 33 | **kwargs 34 | ) 35 | 36 | # plot text 37 | for t in range(len(x)): 38 | if text: 39 | plt.text(x[t][0], x[t][1], r'$x('+str(t)+')$') 40 | 41 | # scatter initial condition 42 | plt.scatter( 43 | x[0][dim[0]], 44 | x[0][dim[1]], 45 | color='w', 46 | edgecolor='k', 47 | zorder=3 48 | ) 49 | 50 | # axis labels 51 | plt.xlabel(r'$x_{' + str(dim[0]+1) + '}$') 52 | plt.ylabel(r'$x_{' + str(dim[1]+1) + '}$') 53 | 54 | def plot_input_sequence(u, h, u_bounds=None): 55 | """ 56 | Plots the input sequence and its bounds as functions of time. 57 | 58 | Arguments 59 | ---------- 60 | u : list of numpy.ndarray 61 | Sequence of the input vectors. 62 | h : float 63 | Time step. 64 | u_bounds : list of numpy.ndarray 65 | Lower and upper bound on the control action. 66 | """ 67 | 68 | # dimension of the input 69 | nu = u[0].shape[0] 70 | 71 | # time axis 72 | N = len(u) 73 | t = np.linspace(0, N*h, N+1) 74 | 75 | # plot each input element separately 76 | for i in range(nu): 77 | plt.subplot(nu, 1, i+1) 78 | 79 | # plot input sequence 80 | u_i_sequence = [u[k][i] for k in range(N)] 81 | input_plot, = plt.step(t, [u_i_sequence[0]] + u_i_sequence, 'b') 82 | 83 | # plot bounds if provided 84 | if u_bounds is not None: 85 | for bound in u_bounds: 86 | bound_plot, = plt.step(t, bound[i]*np.ones(t.shape), 'r') 87 | 88 | # miscellaneous 89 | plt.ylabel(r'$u_{' + str(i+1) + '}$') 90 | plt.xlim((0., N*h)) 91 | if i == 0: 92 | if u_bounds is not None: 93 | plt.legend( 94 | [input_plot, bound_plot], 95 | ['Optimal control', 'Control bounds'], 96 | loc=1 97 | ) 98 | else: 99 | plt.legend( 100 | [input_plot], 101 | ['Optimal control'], 102 | loc=1 103 | ) 104 | plt.xlabel(r'$t$') 105 | 106 | def plot_state_trajectory(x, h, x_bounds=None): 107 | """ 108 | Plots the state trajectory and its bounds as functions of time. 109 | 110 | Arguments 111 | ---------- 112 | x : list of numpy.ndarray 113 | Sequence of the state vectors. 114 | h : float 115 | Time step. 116 | x_bounds : list of numpy.ndarray 117 | Lower and upper bound on the state. 118 | """ 119 | 120 | # dimension of the state 121 | nx = x[0].shape[0] 122 | 123 | # time axis 124 | N = len(x) - 1 125 | t = np.linspace(0, N*h, N+1) 126 | 127 | # plot each state element separately 128 | for i in range(nx): 129 | plt.subplot(nx, 1, i+1) 130 | 131 | # plot state trajectory 132 | x_i_trajectory = [x[k][i] for k in range(N+1)] 133 | state_plot, = plt.plot(t, x_i_trajectory, 'b') 134 | 135 | # plot bounds if provided 136 | if x_bounds is not None: 137 | for bound in x_bounds: 138 | bound_plot, = plt.step(t, bound[i]*np.ones(t.shape),'r') 139 | 140 | # miscellaneous 141 | plt.ylabel(r'$x_{' + str(i+1) + '}$') 142 | plt.xlim((0., N*h)) 143 | if i == 0: 144 | if x_bounds is not None: 145 | plt.legend( 146 | [state_plot, bound_plot], 147 | ['Optimal trajectory', 'State bounds'], 148 | loc=1 149 | ) 150 | else: 151 | plt.legend( 152 | [state_plot], 153 | ['Optimal trajectory'], 154 | loc=1 155 | ) 156 | plt.xlabel(r'$t$') 157 | 158 | def plot_output_trajectory(C, x, h, y_bounds=None): 159 | """ 160 | Plots the output trajectory and its bounds as functions of time. 161 | 162 | Arguments 163 | ---------- 164 | C : numpy.ndarray 165 | Tranformation matrix between the state and the output. 166 | x : list of numpy.ndarray 167 | Sequence of the state vectors. 168 | h : float 169 | Time step. 170 | y_bounds : list of numpy.ndarray 171 | Lower and upper bound on the output. 172 | """ 173 | 174 | # apply linear transformation 175 | y = [C.dot(x_t) for x_t in x] 176 | 177 | # number of plots 178 | ny = C.shape[0] 179 | 180 | # time axis 181 | N = len(x) - 1 182 | t = np.linspace(0, N*h, N+1) 183 | 184 | # plot each state element separately 185 | for i in range(ny): 186 | plt.subplot(ny, 1, i+1) 187 | 188 | # plot state trajectory 189 | y_i_trajectory = [y[k][i] for k in range(N+1)] 190 | output_plot, = plt.plot(t, y_i_trajectory, 'b') 191 | 192 | # plot bounds if provided 193 | if y_bounds is not None: 194 | for bound in y_bounds: 195 | bound_plot, = plt.step(t, bound[i]*np.ones(t.shape),'r') 196 | 197 | # miscellaneous options 198 | plt.ylabel(r'$y_{' + str(i+1) + '}$') 199 | plt.xlim((0., N*h)) 200 | if i == 0: 201 | if y_bounds is not None: 202 | plt.legend( 203 | [output_plot, bound_plot], 204 | ['Optimal trajectory', 'Output bounds'], 205 | loc=1 206 | ) 207 | else: 208 | plt.legend( 209 | [output_plot], 210 | ['Optimal trajectory'], 211 | loc=1 212 | ) 213 | plt.xlabel(r'$t$') -------------------------------------------------------------------------------- /pympc/test/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/pympc/test/__init__.py -------------------------------------------------------------------------------- /pympc/test/test_control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/pympc/test/test_control/__init__.py -------------------------------------------------------------------------------- /pympc/test/test_control/test_controllers.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import unittest 3 | import numpy as np 4 | from copy import copy 5 | 6 | # internal inputs 7 | from pympc.geometry.polyhedron import Polyhedron 8 | from pympc.dynamics.discrete_time_systems import LinearSystem, AffineSystem, PieceWiseAffineSystem 9 | from pympc.control.controllers import ModelPredictiveController, HybridModelPredictiveController 10 | 11 | class testModelPredictiveController(unittest.TestCase): 12 | 13 | def test_implicit_solution(self): 14 | np.random.seed(1) 15 | 16 | # random linear system 17 | for i in range(100): 18 | 19 | # ensure controllability 20 | n = np.random.randint(2, 5) 21 | m = np.random.randint(1, n) 22 | controllable = False 23 | while not controllable: 24 | A = np.random.rand(n,n)/10. 25 | B = np.random.rand(n,m)/10. 26 | S = LinearSystem(A, B) 27 | controllable = S.controllable 28 | 29 | # stage constraints 30 | x_min = -np.random.rand(n) 31 | x_max = np.random.rand(n) 32 | u_min = -np.random.rand(m) 33 | u_max = np.random.rand(m) 34 | X = Polyhedron.from_bounds(x_min, x_max) 35 | U = Polyhedron.from_bounds(u_min, u_max) 36 | D = X.cartesian_product(U) 37 | 38 | # assemble controller 39 | N = np.random.randint(5, 10) 40 | Q = np.eye(n) 41 | R = np.eye(m) 42 | P, K = S.solve_dare(Q, R) 43 | X_N = S.mcais(K, D) 44 | controller = ModelPredictiveController(S, N, Q, R, P, D, X_N) 45 | 46 | # simulate 47 | for j in range(10): 48 | 49 | # intial state 50 | x = np.multiply(np.random.rand(n), x_max - x_min) + x_min 51 | 52 | # mpc 53 | u_mpc, V_mpc = controller.feedforward(x) 54 | 55 | # lqr 56 | V_lqr = .5*x.dot(P).dot(x) 57 | u_lqr = [] 58 | x_next = x 59 | for t in range(N): 60 | u_lqr.append(K.dot(x_next)) 61 | x_next = (A + B.dot(K)).dot(x_next) 62 | 63 | # constraint set (not condensed) 64 | constraints = copy(D) 65 | C = np.hstack((np.eye(n), np.zeros((n,m)))) 66 | constraints.add_equality(C, x) 67 | for t in range(N-1): 68 | constraints = constraints.cartesian_product(D) 69 | C = np.zeros((n, constraints.A.shape[1])) 70 | C[:, -2*(n+m):] = np.hstack((A, B, -np.eye(n), np.zeros((n,m)))) 71 | d = np.zeros(n) 72 | constraints.add_equality(C, d) 73 | constraints = constraints.cartesian_product(X_N) 74 | 75 | # if x is inside mcais lqr = mpc 76 | if X_N.contains(x): 77 | self.assertAlmostEqual(V_mpc, V_lqr) 78 | for t in range(N): 79 | np.testing.assert_array_almost_equal(u_mpc[t], u_lqr[t]) 80 | 81 | # if x is outside mcais lqr > mpc 82 | elif V_mpc is not None: 83 | self.assertTrue(V_mpc > V_lqr) 84 | np.testing.assert_array_almost_equal(u_mpc[0], controller.feedback(x)) 85 | 86 | # simulate the open loop control 87 | x_mpc = S.simulate(x, u_mpc) 88 | for t in range(N): 89 | self.assertTrue(X.contains(x_mpc[t])) 90 | self.assertTrue(U.contains(u_mpc[t])) 91 | self.assertTrue(X_N.contains(x_mpc[N])) 92 | 93 | # check feasibility 94 | xu_mpc = np.concatenate([np.concatenate((x_mpc[t], u_mpc[t])) for t in range(N)]) 95 | xu_mpc = np.concatenate((xu_mpc, x_mpc[N])) 96 | self.assertTrue(constraints.contains(xu_mpc)) 97 | 98 | # else certify empyness of the constraint set 99 | else: 100 | self.assertTrue(controller.feedback(x) is None) 101 | self.assertTrue(constraints.empty) 102 | 103 | def test_explicit_solution(self): 104 | np.random.seed(1) 105 | 106 | # random linear system 107 | for i in range(10): 108 | 109 | # ensure controllability 110 | n = np.random.randint(2, 3) 111 | m = np.random.randint(1, 2) 112 | controllable = False 113 | while not controllable: 114 | A = np.random.rand(n,n) 115 | B = np.random.rand(n,m) 116 | S = LinearSystem(A, B) 117 | controllable = S.controllable 118 | 119 | # stage constraints 120 | x_min = -np.random.rand(n) 121 | x_max = np.random.rand(n) 122 | u_min = -np.random.rand(m) 123 | u_max = np.random.rand(m) 124 | X = Polyhedron.from_bounds(x_min, x_max) 125 | U = Polyhedron.from_bounds(u_min, u_max) 126 | D = X.cartesian_product(U) 127 | 128 | # assemble controller 129 | N = 10 130 | Q = np.eye(n) 131 | R = np.eye(m) 132 | P, K = S.solve_dare(Q, R) 133 | X_N = S.mcais(K, D) 134 | controller = ModelPredictiveController(S, N, Q, R, P, D, X_N) 135 | 136 | # store explicit solution 137 | controller.store_explicit_solution() 138 | 139 | # simulate 140 | for j in range(10): 141 | 142 | # intial state 143 | x = np.multiply(np.random.rand(n), x_max - x_min) + x_min 144 | 145 | # implicit vs explicit mpc 146 | u_implicit, V_implicit = controller.feedforward(x) 147 | u_explicit, V_explicit = controller.feedforward_explicit(x) 148 | 149 | # if feasible 150 | if V_implicit is not None: 151 | self.assertAlmostEqual(V_implicit, V_explicit) 152 | for t in range(N): 153 | np.testing.assert_array_almost_equal(u_implicit[t], u_explicit[t]) 154 | np.testing.assert_array_almost_equal( 155 | controller.feedback(x), 156 | controller.feedback_explicit(x) 157 | ) 158 | 159 | # if unfeasible 160 | else: 161 | self.assertTrue(V_explicit is None) 162 | self.assertTrue(u_explicit is None) 163 | self.assertTrue(controller.feedback_explicit(x) is None) 164 | 165 | class testHybridModelPredictiveController(unittest.TestCase): 166 | 167 | def test_feedforward_feedback_and_get_mpqp(self): 168 | 169 | # numeric parameters 170 | m = 1. 171 | l = 1. 172 | g = 10. 173 | k = 100. 174 | d = .1 175 | h = .01 176 | 177 | # discretization method 178 | method = 'explicit_euler' 179 | 180 | # dynamics n.1 181 | A1 = np.array([[0., 1.],[g/l, 0.]]) 182 | B1 = np.array([[0.],[1/(m*l**2.)]]) 183 | S1 = LinearSystem.from_continuous(A1, B1, h, method) 184 | 185 | # dynamics n.2 186 | A2 = np.array([[0., 1.],[g/l-k/m, 0.]]) 187 | B2 = B1 188 | c2 = np.array([0., k*d/(m*l)]) 189 | S2 = AffineSystem.from_continuous(A2, B2, c2, h, method) 190 | 191 | # list of dynamics 192 | S_list = [S1, S2] 193 | 194 | # state domain n.1 195 | x1_min = np.array([-2.*d/l, -1.5]) 196 | x1_max = np.array([d/l, 1.5]) 197 | X1 = Polyhedron.from_bounds(x1_min, x1_max) 198 | 199 | # state domain n.2 200 | x2_min = np.array([d/l, -1.5]) 201 | x2_max = np.array([2.*d/l, 1.5]) 202 | X2 = Polyhedron.from_bounds(x2_min, x2_max) 203 | 204 | # input domain 205 | u_min = np.array([-4.]) 206 | u_max = np.array([4.]) 207 | U = Polyhedron.from_bounds(u_min, u_max) 208 | 209 | # domains 210 | D1 = X1.cartesian_product(U) 211 | D2 = X2.cartesian_product(U) 212 | D_list = [D1, D2] 213 | 214 | # pwa system 215 | S = PieceWiseAffineSystem(S_list, D_list) 216 | 217 | # controller parameters 218 | N = 20 219 | Q = np.eye(S.nx) 220 | R = np.eye(S.nu) 221 | 222 | # terminal set and cost 223 | P, K = S1.solve_dare(Q, R) 224 | X_N = S1.mcais(K, D1) 225 | 226 | # hybrid MPC controller 227 | controller = HybridModelPredictiveController(S, N, Q, R, P, X_N) 228 | 229 | # compare with lqr 230 | x0 = np.array([.0, .6]) 231 | self.assertTrue(X_N.contains(x0)) 232 | V_lqr = .5*x0.dot(P).dot(x0) 233 | x_lqr = [x0] 234 | u_lqr = [] 235 | for t in range(N): 236 | u_lqr.append(K.dot(x_lqr[t])) 237 | x_lqr.append((S1.A + S1.B.dot(K)).dot(x_lqr[t])) 238 | u_hmpc, x_hmpc, ms_hmpc, V_hmpc = controller.feedforward(x0) 239 | np.testing.assert_array_almost_equal( 240 | np.concatenate((u_lqr)), 241 | np.concatenate((u_hmpc)) 242 | ) 243 | np.testing.assert_array_almost_equal( 244 | np.concatenate((x_lqr)), 245 | np.concatenate((x_hmpc)) 246 | ) 247 | self.assertAlmostEqual(V_lqr, V_hmpc) 248 | self.assertTrue(all([m == 0 for m in ms_hmpc])) 249 | np.testing.assert_array_almost_equal(u_hmpc[0], controller.feedback(x0)) 250 | 251 | # compare with linear mpc 252 | x0 = np.array([.0,.8]) 253 | self.assertFalse(X_N.contains(x0)) 254 | linear_controller = ModelPredictiveController(S1, N, Q, R, P, D1, X_N) 255 | u_lmpc, V_lmpc = linear_controller.feedforward(x0) 256 | x_lmpc = S1.simulate(x0, u_lmpc) 257 | u_hmpc, x_hmpc, ms_hmpc, V_hmpc = controller.feedforward(x0) 258 | np.testing.assert_array_almost_equal( 259 | np.concatenate((u_lmpc)), 260 | np.concatenate((u_hmpc)) 261 | ) 262 | np.testing.assert_array_almost_equal( 263 | np.concatenate((x_lmpc)), 264 | np.concatenate((x_hmpc)) 265 | ) 266 | self.assertAlmostEqual(V_lmpc, V_hmpc) 267 | self.assertTrue(all([m == 0 for m in ms_hmpc])) 268 | np.testing.assert_array_almost_equal(u_hmpc[0], controller.feedback(x0)) 269 | 270 | # test get mpqp 271 | mpqp = controller.get_mpqp(ms_hmpc) 272 | sol = mpqp.solve(x0) 273 | np.testing.assert_array_almost_equal( 274 | np.concatenate((u_lmpc)), 275 | sol['argmin'] 276 | ) 277 | self.assertAlmostEqual(V_lmpc, sol['min']) 278 | 279 | # with change of the mode sequence 280 | x0 = np.array([.09, .2]) 281 | u_hmpc, x_hmpc, ms_hmpc, V_hmpc = controller.feedforward(x0) 282 | self.assertTrue(sum(ms_hmpc) >= 1) 283 | mpqp = controller.get_mpqp(ms_hmpc) 284 | sol = mpqp.solve(x0) 285 | np.testing.assert_array_almost_equal( 286 | np.concatenate((u_hmpc)), 287 | sol['argmin'] 288 | ) 289 | self.assertAlmostEqual(V_hmpc, sol['min']) 290 | 291 | if __name__ == '__main__': 292 | unittest.main() -------------------------------------------------------------------------------- /pympc/test/test_dynamics/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/pympc/test/test_dynamics/__init__.py -------------------------------------------------------------------------------- /pympc/test/test_dynamics/test_discrete_time_systems.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import unittest 3 | import numpy as np 4 | import sympy as sp 5 | 6 | # internal inputs 7 | from pympc.dynamics.discrete_time_systems import LinearSystem, AffineSystem, PieceWiseAffineSystem, mcais 8 | from pympc.geometry.polyhedron import Polyhedron 9 | 10 | class TestLinearSystem(unittest.TestCase): 11 | 12 | def test_intialization(self): 13 | np.random.seed(1) 14 | 15 | # wrong initializations 16 | A = np.ones((3,3)) 17 | B = np.ones((4,1)) 18 | self.assertRaises(ValueError, LinearSystem, A, B) 19 | A = np.ones((4,5)) 20 | self.assertRaises(ValueError, LinearSystem, A, B) 21 | 22 | def test_condense_and_simulate(self): 23 | 24 | # random systems 25 | for i in range(10): 26 | n = np.random.randint(1, 10) 27 | m = np.random.randint(1, 10) 28 | N = np.random.randint(10, 50) 29 | x0 = np.random.rand(n) 30 | u = [np.random.rand(m)/10. for j in range(N)] 31 | A = np.random.rand(n,n)/10. 32 | B = np.random.rand(n,m)/10. 33 | S = LinearSystem(A, B) 34 | 35 | # simulate vs condense 36 | x = S.simulate(x0, u) 37 | A_bar, B_bar = S.condense(N) 38 | np.testing.assert_array_almost_equal( 39 | np.concatenate(x), 40 | A_bar.dot(x0) + B_bar.dot(np.concatenate(u)) 41 | ) 42 | 43 | def test_controllable(self): 44 | 45 | # double integrator (controllable) 46 | A = np.array([[0., 1.],[0., 0.]]) 47 | B = np.array([[0.],[1.]]) 48 | h = .1 49 | S = LinearSystem.from_continuous(A, B, h) 50 | self.assertTrue(S.controllable) 51 | 52 | # make it uncontrollable 53 | B = np.array([[1.],[0.]]) 54 | S = LinearSystem.from_continuous(A, B, h) 55 | self.assertFalse(S.controllable) 56 | 57 | def test_solve_dare_and_simulate_closed_loop(self): 58 | np.random.seed(1) 59 | 60 | # uncontrollable system 61 | A = np.array([[0., 1.],[0., 0.]]) 62 | B = np.array([[1.],[0.]]) 63 | h = .1 64 | S = LinearSystem.from_continuous(A, B, h) 65 | self.assertRaises(ValueError, S.solve_dare, np.eye(2), np.eye(1)) 66 | 67 | # test lqr on random systems 68 | for i in range(100): 69 | n = np.random.randint(5, 10) 70 | m = np.random.randint(1, n -1) 71 | controllable = False 72 | while not controllable: 73 | A = np.random.rand(n,n) 74 | B = np.random.rand(n,m) 75 | S = LinearSystem(A, B) 76 | controllable = S.controllable 77 | Q = np.eye(n) 78 | R = np.eye(m) 79 | P, K = S.solve_dare(Q, R) 80 | self.assertTrue(np.min(np.linalg.eig(P)[0]) > 0.) 81 | 82 | # simulate in closed-loop and check that x' P x is a Lyapunov function 83 | N = 10 84 | x0 = np.random.rand(n) 85 | x_list = S.simulate_closed_loop(x0, N, K) 86 | V_list = [x.dot(P).dot(x) for x in x_list] 87 | dV_list = [V_list[i] - V_list[i+1] for i in range(len(V_list)-1)] 88 | self.assertTrue(min(dV_list) > 0.) 89 | 90 | # simulate in closed-loop and check that 1/2 x' P x is exactly the cost to go 91 | A_cl = A + B.dot(K) 92 | x0 = np.random.rand(n) 93 | infinite_horizon_V = .5*x0.dot(P).dot(x0) 94 | finite_horizon_V = 0. 95 | max_iter = 1000 96 | t = 0 97 | while not np.isclose(infinite_horizon_V, finite_horizon_V): 98 | finite_horizon_V += .5*(x0.dot(Q).dot(x0) + (K.dot(x0)).dot(R).dot(K).dot(x0)) 99 | x0 = A_cl.dot(x0) 100 | t += 1 101 | if t == max_iter: 102 | self.assertTrue(False) 103 | 104 | def test_mcais(self): 105 | """ 106 | Tests only if the function macais() il called correctly. 107 | For the tests of mcais() see the class TestMCAIS. 108 | """ 109 | 110 | # undamped pendulum linearized around the unstable equilibrium 111 | A = np.array([[0., 1.], [1., 0.]]) 112 | B = np.array([[0.], [1.]]) 113 | h = .1 114 | S = LinearSystem.from_continuous(A, B, h) 115 | K = S.solve_dare(np.eye(2), np.eye(1))[1] 116 | d_min = - np.ones(3) 117 | d_max = - d_min 118 | D = Polyhedron.from_bounds(d_min, d_max) 119 | O_inf = S.mcais(K, D) 120 | self.assertTrue(O_inf.contains(np.zeros(2))) 121 | 122 | def test_from_continuous_and_symbolic(self): 123 | 124 | # generate random matrices 125 | h = .01 126 | for i in range(10): 127 | n = np.random.randint(1, 10) 128 | m = np.random.randint(1, 10) 129 | A = np.random.rand(n,n) 130 | B = np.random.rand(n,m) 131 | 132 | # test .from_continuous(), explicit euler vs zoh 133 | convergence = False 134 | while not convergence: 135 | S_ee = LinearSystem.from_continuous(A, B, h, 'explicit_euler') 136 | S_zoh = LinearSystem.from_continuous(A, B, h, 'zero_order_hold') 137 | convergence = np.allclose(S_ee.A, S_zoh.A) and np.allclose(S_ee.B, S_zoh.B) 138 | if not convergence: 139 | h /= 10. 140 | self.assertTrue(convergence) 141 | 142 | # test .from_symbolic() 143 | x = sp.Matrix([sp.Symbol('x'+str(i), real=True) for i in range(n)]) 144 | u = sp.Matrix([sp.Symbol('u'+str(i), real=True) for i in range(m)]) 145 | x_next = sp.Matrix(A)*x + sp.Matrix(B)*u 146 | S = LinearSystem.from_symbolic(x, u, x_next) 147 | np.testing.assert_array_almost_equal(S.A, A) 148 | np.testing.assert_array_almost_equal(S.B, B) 149 | 150 | # test .from_symbolic_continuous() 151 | S = LinearSystem.from_symbolic_continuous(x, u, x_next, h, 'explicit_euler') 152 | np.testing.assert_array_almost_equal(S.A, S_ee.A) 153 | np.testing.assert_array_almost_equal(S.B, S_ee.B) 154 | S = LinearSystem.from_symbolic_continuous(x, u, x_next, h, 'zero_order_hold') 155 | np.testing.assert_array_almost_equal(S.A, S_zoh.A) 156 | np.testing.assert_array_almost_equal(S.B, S_zoh.B) 157 | 158 | # negative time step 159 | self.assertRaises(ValueError, LinearSystem.from_continuous, A, B, -.1, 'explicit_euler') 160 | self.assertRaises(ValueError, LinearSystem.from_symbolic_continuous, x, u, x_next, -.1, 'explicit_euler') 161 | self.assertRaises(ValueError, LinearSystem.from_continuous, A, B, -.1, 'zero_order_hold') 162 | self.assertRaises(ValueError, LinearSystem.from_symbolic_continuous, x, u, x_next, -.1, 'zero_order_hold') 163 | 164 | # wrong input size 165 | x_wrong = sp.Matrix([sp.Symbol('x_wrong'+str(i), real=True) for i in range(n+1)]) 166 | u_wrong = sp.Matrix([sp.Symbol('u_wrong'+str(i), real=True) for i in range(m+1)]) 167 | self.assertRaises(TypeError, LinearSystem.from_symbolic, x_wrong, u, x_next) 168 | self.assertRaises(TypeError, LinearSystem.from_symbolic, x, u_wrong, x_next) 169 | self.assertRaises(TypeError, LinearSystem.from_symbolic_continuous, x_wrong, u, x_next, h, 'explicit_euler') 170 | self.assertRaises(TypeError, LinearSystem.from_symbolic_continuous, x, u_wrong, x_next, h, 'explicit_euler') 171 | self.assertRaises(TypeError, LinearSystem.from_symbolic_continuous, x_wrong, u, x_next, h, 'zero_order_hold') 172 | self.assertRaises(TypeError, LinearSystem.from_symbolic_continuous, x, u_wrong, x_next, h, 'zero_order_hold') 173 | 174 | # from continuous wrong method 175 | self.assertRaises(ValueError, LinearSystem.from_continuous, A, B, h, 'gatto') 176 | self.assertRaises(ValueError, LinearSystem.from_symbolic_continuous, x, u, x_next, h, 'gatto') 177 | 178 | class TestAffineSystem(unittest.TestCase): 179 | 180 | def test_intialization(self): 181 | np.random.seed(1) 182 | 183 | # wrong initializations 184 | A = np.ones((3,3)) 185 | B = np.ones((4,1)) 186 | c = np.ones(4) 187 | self.assertRaises(ValueError, AffineSystem, A, B, c) 188 | A = np.ones((4,5)) 189 | self.assertRaises(ValueError, AffineSystem, A, B, c) 190 | A = np.ones((4,4)) 191 | c = np.ones(5) 192 | self.assertRaises(ValueError, AffineSystem, A, B, c) 193 | 194 | 195 | def test_condense_and_simulate(self): 196 | 197 | # random systems 198 | for i in range(10): 199 | n = np.random.randint(1, 10) 200 | m = np.random.randint(1, 10) 201 | N = np.random.randint(10, 50) 202 | x0 = np.random.rand(n) 203 | u = [np.random.rand(m)/10. for j in range(N)] 204 | A = np.random.rand(n,n)/10. 205 | B = np.random.rand(n,m)/10. 206 | c = np.random.rand(n)/10. 207 | S = AffineSystem(A, B, c) 208 | 209 | # simulate vs condense 210 | x = S.simulate(x0, u) 211 | A_bar, B_bar, c_bar = S.condense(N) 212 | np.testing.assert_array_almost_equal( 213 | np.concatenate(x), 214 | A_bar.dot(x0) + B_bar.dot(np.concatenate(u)) + c_bar 215 | ) 216 | 217 | def test_from_continuous_and_symbolic(self): 218 | 219 | # generate random matrices 220 | h = .01 221 | for i in range(10): 222 | n = np.random.randint(1, 10) 223 | m = np.random.randint(1, 10) 224 | A = np.random.rand(n,n) 225 | B = np.random.rand(n,m) 226 | c = np.random.rand(n) 227 | 228 | # test .from_continuous(), explicit euler vs zoh 229 | convergence = False 230 | while not convergence: 231 | S_ee = AffineSystem.from_continuous(A, B, c, h, 'explicit_euler') 232 | S_zoh = AffineSystem.from_continuous(A, B, c, h, 'zero_order_hold') 233 | convergence = np.allclose(S_ee.A, S_zoh.A) and np.allclose(S_ee.B, S_zoh.B) and np.allclose(S_ee.c, S_zoh.c) 234 | if not convergence: 235 | h /= 10. 236 | self.assertTrue(convergence) 237 | 238 | # test .from_symbolic() 239 | x = sp.Matrix([sp.Symbol('x'+str(i), real=True) for i in range(n)]) 240 | u = sp.Matrix([sp.Symbol('u'+str(i), real=True) for i in range(m)]) 241 | x_next = sp.Matrix(A)*x + sp.Matrix(B)*u + sp.Matrix(c) 242 | S = AffineSystem.from_symbolic(x, u, x_next) 243 | np.testing.assert_array_almost_equal(S.A, A) 244 | np.testing.assert_array_almost_equal(S.B, B) 245 | np.testing.assert_array_almost_equal(S.c, c) 246 | 247 | # test .from_symbolic_continuous() 248 | S = AffineSystem.from_symbolic_continuous(x, u, x_next, h, 'explicit_euler') 249 | np.testing.assert_array_almost_equal(S.A, S_ee.A) 250 | np.testing.assert_array_almost_equal(S.B, S_ee.B) 251 | np.testing.assert_array_almost_equal(S.c, S_ee.c) 252 | S = AffineSystem.from_symbolic_continuous(x, u, x_next, h, 'zero_order_hold') 253 | np.testing.assert_array_almost_equal(S.A, S_zoh.A) 254 | np.testing.assert_array_almost_equal(S.B, S_zoh.B) 255 | np.testing.assert_array_almost_equal(S.c, S_zoh.c) 256 | 257 | # negative time step 258 | self.assertRaises(ValueError, AffineSystem.from_continuous, A, B, c, -.1, 'explicit_euler') 259 | self.assertRaises(ValueError, AffineSystem.from_symbolic_continuous, x, u, x_next, -.1, 'explicit_euler') 260 | self.assertRaises(ValueError, AffineSystem.from_continuous, A, B, c, -.1, 'zero_order_hold') 261 | self.assertRaises(ValueError, AffineSystem.from_symbolic_continuous, x, u, x_next, -.1, 'zero_order_hold') 262 | 263 | # wrong input size 264 | x_wrong = sp.Matrix([sp.Symbol('x_wrong'+str(i), real=True) for i in range(n+1)]) 265 | u_wrong = sp.Matrix([sp.Symbol('u_wrong'+str(i), real=True) for i in range(m+1)]) 266 | self.assertRaises(TypeError, AffineSystem.from_symbolic, x_wrong, u, x_next) 267 | self.assertRaises(TypeError, AffineSystem.from_symbolic, x, u_wrong, x_next) 268 | self.assertRaises(TypeError, AffineSystem.from_symbolic_continuous, x_wrong, u, x_next, h, 'explicit_euler') 269 | self.assertRaises(TypeError, AffineSystem.from_symbolic_continuous, x, u_wrong, x_next, h, 'explicit_euler') 270 | self.assertRaises(TypeError, AffineSystem.from_symbolic_continuous, x_wrong, u, x_next, h, 'zero_order_hold') 271 | self.assertRaises(TypeError, AffineSystem.from_symbolic_continuous, x, u_wrong, x_next, h, 'zero_order_hold') 272 | 273 | # from continuous wrong method 274 | self.assertRaises(ValueError, AffineSystem.from_continuous, A, B, c, h, 'gatto') 275 | self.assertRaises(ValueError, AffineSystem.from_symbolic_continuous, x, u, x_next, h, 'gatto') 276 | 277 | class TestPieceWiseAffineSystem(unittest.TestCase): 278 | 279 | def test_intialization(self): 280 | np.random.seed(1) 281 | 282 | # different number of systems and domains 283 | A = np.ones((3,3)) 284 | B = np.ones((3,2)) 285 | c = np.ones(3) 286 | S = AffineSystem(A, B, c) 287 | affine_systems = [S]*5 288 | F = np.ones((9,5)) 289 | g = np.ones(9) 290 | D = Polyhedron(F, g) 291 | domains = [D]*4 292 | self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems, domains) 293 | 294 | # incopatible number of states in affine systems 295 | domains += [D, D] 296 | A = np.ones((2,2)) 297 | B = np.ones((2,2)) 298 | c = np.ones(2) 299 | affine_systems.append(AffineSystem(A, B, c)) 300 | self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems, domains) 301 | 302 | # incopatible number of inputs in affine systems 303 | del affine_systems[-1] 304 | A = np.ones((3,3)) 305 | B = np.ones((3,1)) 306 | c = np.ones(3) 307 | affine_systems.append(AffineSystem(A, B, c)) 308 | self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems, domains) 309 | 310 | # different dimensinality of the domains and the systems 311 | del affine_systems[-1] 312 | affine_systems += [S, S] 313 | F = np.ones((9,4)) 314 | g = np.ones(9) 315 | domains.append(Polyhedron(F, g)) 316 | self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems, domains) 317 | 318 | # different dimensinality of the domains and the systems 319 | F = np.ones((9,4)) 320 | g = np.ones(9) 321 | D = Polyhedron(F, g) 322 | domains = [D]*7 323 | self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems, domains) 324 | 325 | def test_condense_and_simulate_and_get_mode(self): 326 | np.random.seed(1) 327 | 328 | # test with random systems 329 | for i in range(10): 330 | 331 | # test dimensions 332 | n = np.random.randint(1, 10) 333 | m = np.random.randint(1, 10) 334 | N = np.random.randint(10, 50) 335 | 336 | # initial state 337 | x0 = np.random.rand(n) 338 | 339 | # initialize loop variables 340 | x_t = x0 341 | x = x0 342 | u = np.zeros(0) 343 | u_list = [] 344 | affine_systems = [] 345 | domains = [] 346 | 347 | # simulate for N steps 348 | for t in range(N): 349 | 350 | # generate random dynamics 351 | A_t = np.random.rand(n,n)/10. 352 | B_t = np.random.rand(n,m)/10. 353 | c_t = np.random.rand(n)/10. 354 | 355 | # simulate with random input 356 | u_t = np.random.rand(m)/10. 357 | u = np.concatenate((u, u_t)) 358 | u_list.append(u_t) 359 | x_tp1 = A_t.dot(x_t) + B_t.dot(u_t) + c_t 360 | x = np.concatenate((x, x_tp1)) 361 | 362 | # create a domain that contains x and u (it has to be super-tight so that the pwa system is well posed!) 363 | D = Polyhedron.from_bounds( 364 | np.concatenate((x_t/1.01, u_t/1.01)), 365 | np.concatenate((x_t*1.01, u_t*1.01)) 366 | ) 367 | domains.append(D) 368 | affine_systems.append(AffineSystem(A_t, B_t, c_t)) 369 | 370 | # reset state 371 | x_t = x_tp1 372 | 373 | # construct pwa system 374 | S = PieceWiseAffineSystem(affine_systems, domains) 375 | 376 | # test condense 377 | mode_sequence = range(N) 378 | A_bar, B_bar, c_bar = S.condense(mode_sequence) 379 | np.testing.assert_array_almost_equal( 380 | x, 381 | A_bar.dot(x0) + B_bar.dot(u) + c_bar 382 | ) 383 | 384 | # test simulate 385 | x_list, mode_sequence = S.simulate(x0, u_list) 386 | np.testing.assert_array_almost_equal(x, np.concatenate(x_list)) 387 | 388 | # test get mode 389 | for t in range(N): 390 | self.assertTrue(S.get_mode(x_list[t], u_list[t]) == t) 391 | 392 | def test_is_well_posed(self): 393 | 394 | # domains 395 | D0 = Polyhedron.from_bounds(-np.ones(3), np.ones(3)) 396 | D1 = Polyhedron.from_bounds(np.zeros(3), 2.*np.ones(3)) 397 | domains = [D0, D1] 398 | 399 | # pwa system 400 | A = np.ones((2,2)) 401 | B = np.ones((2,1)) 402 | c = np.ones(2) 403 | S = AffineSystem(A, B, c) 404 | affine_systems = [S]*2 405 | S_pwa = PieceWiseAffineSystem(affine_systems, domains) 406 | 407 | # check if well posed 408 | self.assertFalse(S_pwa.is_well_posed()) 409 | 410 | # make it well posed 411 | D1.add_lower_bound(np.ones(3)) 412 | D2 = Polyhedron.from_bounds(2.*np.ones(3), 3.*np.ones(3)) 413 | domains = [D0, D1, D2] 414 | affine_systems = [S]*3 415 | S_pwa = PieceWiseAffineSystem(affine_systems, domains) 416 | self.assertTrue(S_pwa.is_well_posed()) 417 | 418 | class TestMCAIS(unittest.TestCase): 419 | 420 | def test_mcais(self): 421 | np.random.seed(1) 422 | 423 | # domain 424 | nx = 2 425 | x_min = - np.ones(nx) 426 | x_max = - x_min 427 | X = Polyhedron.from_bounds(x_min, x_max) 428 | 429 | # stable dynamics 430 | for i in range(10): 431 | stable = False 432 | while not stable: 433 | A = np.random.rand(nx, nx) 434 | stable = np.max(np.absolute(np.linalg.eig(A)[0])) < 1. 435 | 436 | # get mcais 437 | O_inf = mcais(A, X) 438 | 439 | # generate random initial conditions 440 | for j in range(100): 441 | x = 3.*np.random.rand(nx) - 1.5 442 | 443 | # if inside stays inside X, if outside sooner or later will leave X 444 | if O_inf.contains(x): 445 | while np.linalg.norm(x) > 0.001: 446 | x = A.dot(x) 447 | self.assertTrue(X.contains(x)) 448 | else: 449 | while X.contains(x): 450 | x = A.dot(x) 451 | if np.linalg.norm(x) < 0.0001: 452 | self.assertTrue(False) 453 | pass 454 | 455 | if __name__ == '__main__': 456 | unittest.main() -------------------------------------------------------------------------------- /pympc/test/test_dynamics/test_discretization_methods.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import unittest 3 | import numpy as np 4 | 5 | # internal inputs 6 | from pympc.dynamics.discretization_methods import explicit_euler, zero_order_hold 7 | 8 | class TestDiscretizationMethods(unittest.TestCase): 9 | 10 | def test_explicit_euler_and_zero_order_hold(self): 11 | np.random.seed(1) 12 | 13 | # double integrator 14 | A = np.array([[0., 1.],[0., 0.]]) 15 | B = np.array([[0.],[1.]]) 16 | c = np.zeros(2) 17 | h = 1. 18 | 19 | # discrete time from continuous 20 | A_d, B_d, c_d = explicit_euler(A, B, c, h) 21 | np.testing.assert_array_almost_equal(A_d, np.eye(2) + A*h) 22 | np.testing.assert_array_almost_equal(B_d, B*h) 23 | np.testing.assert_array_almost_equal(c_d, c) 24 | 25 | # discrete time from continuous 26 | A_d, B_d, c_d = zero_order_hold(A, B, c, h) 27 | np.testing.assert_array_almost_equal(A_d, np.eye(2) + A*h) 28 | np.testing.assert_array_almost_equal( 29 | B_d, 30 | B*h + np.array([[0.,h**2/2.],[0.,0.]]).dot(B) 31 | ) 32 | np.testing.assert_array_almost_equal(c_d, c) 33 | 34 | # test with random systems 35 | for i in range(100): 36 | n = np.random.randint(1, 10) 37 | m = np.random.randint(1, 10) 38 | A = np.random.rand(n,n) 39 | B = np.random.rand(n,m) 40 | c = np.random.rand(n) 41 | 42 | # reduce discretization step until the two method are almost equivalent 43 | h = .01 44 | convergence = False 45 | while not convergence: 46 | A_ee, B_ee, c_ee = explicit_euler(A, B, c, h) 47 | A_zoh, B_zoh, c_zoh = zero_order_hold(A, B, c, h) 48 | convergence = np.allclose(A_ee, A_zoh) and np.allclose(B_ee, B_zoh) and np.allclose(c_ee, c_zoh) 49 | if not convergence: 50 | h /= 10. 51 | self.assertTrue(convergence) 52 | 53 | if __name__ == '__main__': 54 | unittest.main() -------------------------------------------------------------------------------- /pympc/test/test_dynamics/test_utils.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import unittest 3 | import numpy as np 4 | 5 | # internal inputs 6 | from pympc.dynamics.utils import check_affine_system 7 | 8 | class TestUtils(unittest.TestCase): 9 | 10 | def test_check_affine_system(self): 11 | 12 | # non-square A 13 | A = np.ones((2,1)) 14 | B = np.ones((2,1)) 15 | self.assertRaises(ValueError, check_affine_system, A, B) 16 | 17 | # uncoherent B and A 18 | A = np.ones((3,3)) 19 | self.assertRaises(ValueError, check_affine_system, A, B) 20 | 21 | # uncoherent c and A 22 | B = np.ones((3,1)) 23 | c = np.ones(1) 24 | self.assertRaises(ValueError, check_affine_system, A, B, c) 25 | 26 | # 2-dimensional c 27 | c = np.ones((3,1)) 28 | self.assertRaises(ValueError, check_affine_system, A, B, c) 29 | 30 | # negative h 31 | c = np.ones(3) 32 | h = -.1 33 | self.assertRaises(ValueError, check_affine_system, A, B, c, h) 34 | 35 | if __name__ == '__main__': 36 | unittest.main() -------------------------------------------------------------------------------- /pympc/test/test_geometry/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/pympc/test/test_geometry/__init__.py -------------------------------------------------------------------------------- /pympc/test/test_geometry/test_utils.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | from six.moves import range # behaves like xrange on python2, range on python3 3 | import unittest 4 | import numpy as np 5 | from random import shuffle 6 | 7 | # internal inputs 8 | from pympc.geometry.utils import nullspace_basis, linearly_independent_rows, plane_through_points, same_rows, same_vectors 9 | 10 | class TestUtils(unittest.TestCase): 11 | 12 | def test_nullspace_basis(self): 13 | 14 | # empty nullspace 15 | A = np.eye(3) 16 | N = nullspace_basis(A) 17 | self.assertTrue(N.shape == (3,0)) 18 | 19 | # 1d nullspace 20 | A = np.array([[1.,0.,0.],[0.,1.,0.]]) 21 | N = nullspace_basis(A) 22 | np.testing.assert_array_almost_equal( 23 | N, 24 | np.array([[0.],[0.],[1.]]) 25 | ) 26 | 27 | # 1d nullspace 28 | A = np.array([[1.,0.,0.],[0.,1.,1.]]) 29 | N = nullspace_basis(A) 30 | self.assertTrue(N.shape == (3,1)) 31 | self.assertAlmostEqual(N[0,0], 0.) 32 | self.assertAlmostEqual(N[1,0]/N[2,0], -1.) 33 | 34 | def test_linearly_independent_rows(self): 35 | 36 | # one linear dependency 37 | A = np.array([[1.,0.,0.],[0.,1.,0.],[1.,0.,0.]]) 38 | li_rows = linearly_independent_rows(A) 39 | self.assertTrue(len(li_rows) == 2) 40 | self.assertTrue(1 in li_rows) 41 | li_rows.remove(1) 42 | self.assertTrue(li_rows[0] == 0 or li_rows[0] == 2) 43 | 44 | # all linearly independent 45 | A = np.eye(3) 46 | li_rows = linearly_independent_rows(A) 47 | self.assertTrue(li_rows == list(range(3))) 48 | 49 | def test_plane_through_points(self): 50 | 51 | # n-dimensional case 52 | for n in range(2,10): 53 | points = [p for p in np.eye(n)] 54 | a, d = plane_through_points(points) 55 | np.testing.assert_array_almost_equal( 56 | a, 57 | np.ones(n)/np.sqrt(n) 58 | ) 59 | self.assertAlmostEqual(d, 1./np.sqrt(n)) 60 | 61 | # 2d case through origin 62 | points = [np.array([1.,-1.]), np.array([-1.,1.])] 63 | a, d = plane_through_points(points) 64 | self.assertAlmostEqual(a[0]/a[1], 1.) 65 | self.assertAlmostEqual(d, 0.) 66 | 67 | def test_same_rows(self): 68 | np.random.seed(1) 69 | 70 | # test dimensions 71 | n = 10 72 | m = 5 73 | for i in range(10): 74 | 75 | # check with scaling factors 76 | A = np.random.rand(n, m) 77 | scaling = np.random.rand(n) 78 | B = np.multiply(A.T, scaling).T 79 | self.assertTrue(same_rows(A, B)) 80 | 81 | # check without scaling factors 82 | B_order = list(range(n)) 83 | shuffle(B_order) 84 | B = A[B_order, :] 85 | self.assertTrue(same_rows(A, B, normalize=False)) 86 | 87 | # wrong check (same columns) 88 | scaling = np.random.rand(m) 89 | B = np.multiply(A, scaling) 90 | self.assertFalse(same_rows(A, B)) 91 | 92 | 93 | def test_same_vectors(self): 94 | np.random.seed(1) 95 | 96 | # test dimensions 97 | n = 10 98 | N = 100 99 | for i in range(10): 100 | 101 | # check equal lists 102 | v_list = [np.random.rand(n) for j in range(N)] 103 | u_order = list(range(N)) 104 | shuffle(u_order) 105 | u_list = [v_list[j] for j in u_order] 106 | self.assertTrue(same_vectors(v_list, u_list)) 107 | 108 | # wrong size (only 1d arrays allowed) 109 | v_list = [np.random.rand(n, 1) for j in range(N)] 110 | u_order = list(range(N)) 111 | shuffle(u_order) 112 | u_list = [v_list[j] for j in u_order] 113 | self.assertRaises(ValueError, same_vectors, v_list, u_list) 114 | 115 | if __name__ == '__main__': 116 | unittest.main() -------------------------------------------------------------------------------- /pympc/test/test_optimization/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TobiaMarcucci/pympc/557c557c9f3a2810b194aef9ca6401f72a3babfd/pympc/test/test_optimization/__init__.py -------------------------------------------------------------------------------- /pympc/test/test_optimization/test_parametric_programs.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import unittest 3 | import numpy as np 4 | 5 | # internal inputs 6 | from pympc.optimization.parametric_programs import MultiParametricQuadraticProgram 7 | from pympc.geometry.utils import same_rows 8 | 9 | class TestMultiParametricQuadraticProgram(unittest.TestCase): 10 | """ 11 | These are just some very basic tests. 12 | The class MultiParametricQuadraticProgram is further tested with the ModelPredictiveController class. 13 | """ 14 | 15 | def test_solve(self): 16 | 17 | # 1-dimensional mpqp 18 | H = dict() 19 | H['uu'] = np.array([[1.]]) 20 | H['xx'] = np.zeros((1,1)) 21 | H['ux'] = np.zeros((1,1)) 22 | f = dict() 23 | f['u'] = np.zeros(1) 24 | f['x'] = np.zeros(1) 25 | g = 0. 26 | A = dict() 27 | A['u'] = np.array([[1.],[-1.],[0.],[0.]]) 28 | A['x'] = np.array([[-1.],[1.],[1.],[-1.]]) 29 | b = np.array([1.,1.,2.,2.]) 30 | mpqp = MultiParametricQuadraticProgram(H, f, g, A, b) 31 | 32 | # explicit solve given active set 33 | cr = mpqp.explicit_solve_given_active_set([]) 34 | self.assertAlmostEqual(cr._V['xx'], 0.) 35 | self.assertAlmostEqual(cr._V['x'], 0.) 36 | self.assertAlmostEqual(cr._V['0'], 0.) 37 | self.assertAlmostEqual(cr._u['x'], 0.) 38 | self.assertAlmostEqual(cr._u['0'], 0.) 39 | cr = mpqp.explicit_solve_given_active_set([0]) 40 | self.assertAlmostEqual(cr._V['xx'], 1.) 41 | self.assertAlmostEqual(cr._V['x'], 1.) 42 | self.assertAlmostEqual(cr._V['0'], .5) 43 | self.assertAlmostEqual(cr._u['x'], 1.) 44 | self.assertAlmostEqual(cr._u['0'], 1.) 45 | 46 | # explicit solve given point 47 | cr = mpqp.explicit_solve_given_point(np.array([1.5])) 48 | self.assertAlmostEqual(cr._V['xx'], 1.) 49 | self.assertAlmostEqual(cr._V['x'], -1.) 50 | self.assertAlmostEqual(cr._V['0'], .5) 51 | self.assertAlmostEqual(cr._u['x'], 1.) 52 | self.assertAlmostEqual(cr._u['0'], -1.) 53 | 54 | # implicit solve given point 55 | sol = mpqp.solve(np.array([1.5])) 56 | self.assertAlmostEqual(sol['min'], .125) 57 | self.assertTrue(np.allclose(sol['argmin'], 0.5)) 58 | self.assertEqual(sol['active_set'], [1]) 59 | 60 | # solve 61 | exp_sol = mpqp.explicit_solve() 62 | for x in [np.array([.5]), np.array([1.5]), np.array([-1.5]), np.array([2.5]), np.array([-2.5])]: 63 | sol = mpqp.solve(x) 64 | if sol['min'] is not None: 65 | self.assertAlmostEqual(sol['min'], exp_sol.V(x)) 66 | np.testing.assert_array_almost_equal(sol['argmin'], exp_sol.u(x)) 67 | np.testing.assert_array_almost_equal(sol['multiplier_inequality'], exp_sol.p(x)) 68 | else: 69 | self.assertTrue(exp_sol.V(x) is None) 70 | self.assertTrue(exp_sol.u(x) is None) 71 | self.assertTrue(exp_sol.p(x) is None) 72 | 73 | # feasible set 74 | fs = mpqp.get_feasible_set() 75 | A = np.array([[1.],[-1.]]) 76 | b = np.array([[2.],[2.]]) 77 | self.assertTrue(same_rows( 78 | np.column_stack((A, b)), 79 | np.column_stack((fs.A, fs.b)) 80 | )) 81 | 82 | if __name__ == '__main__': 83 | unittest.main() 84 | -------------------------------------------------------------------------------- /pympc/test/test_optimization/test_programs.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import unittest 3 | import numpy as np 4 | 5 | # internal inputs 6 | from pympc.optimization.programs import linear_program, quadratic_program, mixed_integer_quadratic_program 7 | 8 | class TestPrograms(unittest.TestCase): 9 | """ 10 | This tests only if the solvers are called correctly,. 11 | More tests on the correctness of the results can be fuond in test_solvers.py. 12 | """ 13 | 14 | def test_linear_program(self, solver='pnnls'): 15 | 16 | # trivial LP with only inequalities 17 | A = -np.eye(2) 18 | b = np.zeros(2) 19 | f = np.ones(2) 20 | sol = linear_program(f, A, b, solver=solver) 21 | self.assertAlmostEqual( 22 | sol['min'], 23 | 0. 24 | ) 25 | np.testing.assert_array_almost_equal( 26 | sol['argmin'], 27 | np.zeros(2) 28 | ) 29 | self.assertEqual( 30 | sol['active_set'], 31 | [0,1] 32 | ) 33 | np.testing.assert_array_almost_equal( 34 | sol['multiplier_inequality'], 35 | np.ones(2) 36 | ) 37 | self.assertTrue( 38 | sol['multiplier_equality'] is None 39 | ) 40 | 41 | # add equality 42 | C = np.array([[2., 1.]]) 43 | d = np.array([2.]) 44 | sol = linear_program(f, A, b, C, d, solver=solver) 45 | self.assertAlmostEqual( 46 | sol['min'], 47 | 1. 48 | ) 49 | np.testing.assert_array_almost_equal( 50 | sol['argmin'], 51 | np.array([1.,0.]) 52 | ) 53 | self.assertEqual( 54 | sol['active_set'], 55 | [1] 56 | ) 57 | np.testing.assert_array_almost_equal( 58 | sol['multiplier_inequality'], 59 | np.array([0.,.5]) 60 | ) 61 | np.testing.assert_array_almost_equal( 62 | sol['multiplier_equality'], 63 | np.array([-.5]) 64 | ) 65 | 66 | def test_linear_program_gurobi(self): 67 | 68 | # use gurobi to solve the mathematical programs 69 | self.test_linear_program(solver='gurobi') 70 | 71 | def test_quadratic_program(self, solver='pnnls'): 72 | 73 | # trivial QP with only inequalities 74 | H = np.eye(2) 75 | f = np.ones(2) 76 | A = -np.eye(2) 77 | b = -np.ones(2) 78 | sol = quadratic_program(H, f, A, b, solver=solver) 79 | self.assertAlmostEqual( 80 | sol['min'], 81 | 3. 82 | ) 83 | np.testing.assert_array_almost_equal( 84 | sol['argmin'], 85 | np.array([1.,1.]) 86 | ) 87 | self.assertEqual( 88 | sol['active_set'], 89 | [0,1] 90 | ) 91 | np.testing.assert_array_almost_equal( 92 | sol['multiplier_inequality'], 93 | np.array([2.,2.]) 94 | ) 95 | self.assertTrue( 96 | sol['multiplier_equality'] is None 97 | ) 98 | 99 | # add equality constraints 100 | C = np.array([[0., 1.]]) 101 | d = np.array([2.]) 102 | sol = quadratic_program(H, f, A, b, C, d, solver=solver) 103 | self.assertAlmostEqual( 104 | sol['min'], 105 | 5.5 106 | ) 107 | np.testing.assert_array_almost_equal( 108 | sol['argmin'], 109 | np.array([1.,2.]) 110 | ) 111 | self.assertEqual( 112 | sol['active_set'], 113 | [0] 114 | ) 115 | np.testing.assert_array_almost_equal( 116 | sol['multiplier_inequality'], 117 | np.array([2.,0.]) 118 | ) 119 | np.testing.assert_array_almost_equal( 120 | sol['multiplier_equality'], 121 | np.array([-3.]) 122 | ) 123 | 124 | def test_quadratic_program_gurobi(self): 125 | 126 | # use gurobi to solve the mathematical programs 127 | self.test_quadratic_program(solver='gurobi') 128 | 129 | def test_mixed_integer_quadratic_program_gurobi(self): 130 | 131 | # select solvers 132 | solver = 'gurobi' 133 | 134 | # simple miqp 135 | H = np.eye(2) 136 | f = np.array([[0.],[-.6]]) 137 | nc = 1 138 | A = np.eye(2) 139 | b = 2.*np.ones((2,1)) 140 | sol = mixed_integer_quadratic_program(nc, H, f, A, b, solver=solver) 141 | self.assertAlmostEqual( 142 | sol['min'], 143 | -.1 144 | ) 145 | np.testing.assert_array_almost_equal( 146 | sol['argmin'], 147 | np.array([0.,1.]) 148 | ) 149 | 150 | # add equalities 151 | C = np.array([[1., -1.]]) 152 | d = np.zeros((1,1)) 153 | sol = mixed_integer_quadratic_program(nc, H, f, A, b, C, d, solver=solver) 154 | self.assertAlmostEqual( 155 | sol['min'], 156 | 0. 157 | ) 158 | np.testing.assert_array_almost_equal( 159 | sol['argmin'], 160 | np.zeros(2) 161 | ) 162 | 163 | # unfeasible miqp 164 | C = np.ones((1,2)) 165 | d = 5.*np.ones((1,1)) 166 | sol = mixed_integer_quadratic_program(nc, H, f, A, b, C, d, solver=solver) 167 | self.assertTrue(sol['min'] is None) 168 | self.assertTrue(sol['argmin'] is None) 169 | 170 | if __name__ == '__main__': 171 | unittest.main() -------------------------------------------------------------------------------- /pympc/test/test_optimization/test_solvers.py: -------------------------------------------------------------------------------- 1 | # external imports 2 | import unittest 3 | import numpy as np 4 | 5 | # internal inputs 6 | from pympc.optimization.solvers.pnnls import linear_program as lp_pnnls, quadratic_program as qp_pnnls 7 | from pympc.optimization.solvers.gurobi import linear_program as lp_gurobi, quadratic_program as qp_gurobi, mixed_integer_quadratic_program as miqp_gurobi 8 | # from pympc.optimization.solvers.drake import linear_program as lp_drake, quadratic_program as qp_drake, mixed_integer_quadratic_program as miqp_drake 9 | 10 | class TestSolvers(unittest.TestCase): 11 | 12 | def test_linear_programming_solver(self, linear_program=lp_pnnls): 13 | 14 | # trivial lp with only inequalities 15 | f = np.ones(2) 16 | A = np.array([[1.,0.],[-1.,0.],[0.,1.],[0.,-1.]]) 17 | b = np.array([5.,5.,1.,1.]) 18 | sol = linear_program(f, A, b) 19 | self.assertAlmostEqual( 20 | sol['min'], 21 | -6. 22 | ) 23 | np.testing.assert_array_almost_equal( 24 | sol['argmin'], 25 | np.array([-5.,-1.]) 26 | ) 27 | self.assertEqual( 28 | sol['active_set'], 29 | [1,3] 30 | ) 31 | np.testing.assert_array_almost_equal( 32 | sol['multiplier_inequality'], 33 | np.array([0.,1.,0.,1.]) 34 | ) 35 | self.assertTrue( 36 | sol['multiplier_equality'] is None 37 | ) 38 | 39 | # another trivial LP with only inequalities 40 | A = -np.eye(2) 41 | b = np.zeros(2) 42 | sol = linear_program(f, A, b) 43 | self.assertAlmostEqual( 44 | sol['min'], 45 | 0. 46 | ) 47 | np.testing.assert_array_almost_equal( 48 | sol['argmin'], 49 | np.zeros(2) 50 | ) 51 | self.assertEqual( 52 | sol['active_set'], 53 | [0,1] 54 | ) 55 | np.testing.assert_array_almost_equal( 56 | sol['multiplier_inequality'], 57 | np.ones(2) 58 | ) 59 | self.assertTrue( 60 | sol['multiplier_equality'] is None 61 | ) 62 | 63 | # add equality 64 | C = np.array([[2., 1.]]) 65 | d = np.array([2.]) 66 | sol = linear_program(f, A, b, C, d) 67 | self.assertAlmostEqual( 68 | sol['min'], 69 | 1. 70 | ) 71 | np.testing.assert_array_almost_equal( 72 | sol['argmin'], 73 | np.array([1.,0.]) 74 | ) 75 | self.assertEqual( 76 | sol['active_set'], 77 | [1] 78 | ) 79 | np.testing.assert_array_almost_equal( 80 | sol['multiplier_inequality'], 81 | np.array([0.,.5]) 82 | ) 83 | np.testing.assert_array_almost_equal( 84 | sol['multiplier_equality'], 85 | np.array([-.5]) 86 | ) 87 | 88 | # unfeasible lp 89 | A = np.array([[0.,1.],[0.,-1.]]) 90 | b = np.array([0.,-1.]) 91 | sol = linear_program(f, A, b) 92 | for value in sol.values(): 93 | self.assertTrue(value is None) 94 | 95 | # unbounded lp 96 | A = np.array([[1.,0.],[0.,1.],[0.,-1.]]) 97 | b = np.array([0.,0.,1.]) 98 | sol = linear_program(f, A, b) 99 | for value in sol.values(): 100 | self.assertTrue(value is None) 101 | 102 | # add equalities (still unbounded) 103 | C = np.array([[0., 1.]]) 104 | d = np.array([-.1]) 105 | sol = linear_program(f, A, b, C, d) 106 | for value in sol.values(): 107 | self.assertTrue(value is None) 108 | 109 | # add equalities (now bounded) 110 | C = np.array([[1., -1.]]) 111 | d = np.array([0.]) 112 | sol = linear_program(f, A, b, C, d) 113 | self.assertAlmostEqual( 114 | sol['min'], 115 | -2. 116 | ) 117 | np.testing.assert_array_almost_equal( 118 | sol['argmin'], 119 | - np.ones(2) 120 | ) 121 | self.assertEqual( 122 | sol['active_set'], 123 | [2] 124 | ) 125 | np.testing.assert_array_almost_equal( 126 | sol['multiplier_inequality'], 127 | np.array([0.,0.,2.]) 128 | ) 129 | np.testing.assert_array_almost_equal( 130 | sol['multiplier_equality'], 131 | np.array([-1.]) 132 | ) 133 | 134 | # bounded LP with unbounded domain 135 | f = np.array([0.,1.]) 136 | A = np.array([[0.,-1.]]) 137 | b = np.zeros(1) 138 | sol = linear_program(f, A, b) 139 | self.assertAlmostEqual( 140 | sol['min'], 141 | 0. 142 | ) 143 | self.assertAlmostEqual( 144 | sol['argmin'][1], 145 | 0. 146 | ) 147 | self.assertEqual( 148 | sol['active_set'], 149 | [0] 150 | ) 151 | np.testing.assert_array_almost_equal( 152 | sol['multiplier_inequality'], 153 | np.array([1.]) 154 | ) 155 | self.assertTrue( 156 | sol['multiplier_equality'] is None 157 | ) 158 | 159 | # 3d LPs (constraint set: min_x -x_1 s.t. ||x||_1 <= 1, x_1 <= .5) 160 | A = np.array([ 161 | [1., 1., 1.], 162 | [-1., 1., 1.], 163 | [1., -1., 1.], 164 | [1., 1., -1.], 165 | [-1., -1., 1.], 166 | [1., -1., -1.], 167 | [-1., 1., -1.], 168 | [-1., -1., -1.], 169 | [1., 0., 0.] 170 | ]) 171 | b = np.concatenate((np.ones(8), np.array([.5]))) 172 | f = np.array([-1.,0.,0.]) 173 | sol = linear_program(f, A, b) 174 | self.assertAlmostEqual( 175 | sol['min'], 176 | -.5 177 | ) 178 | self.assertAlmostEqual( 179 | sol['argmin'][0], 180 | .5 181 | ) 182 | self.assertTrue(8 in sol['active_set']) 183 | np.testing.assert_array_almost_equal( 184 | sol['multiplier_inequality'], 185 | np.concatenate((np.zeros(8), np.ones(1))) 186 | ) 187 | self.assertTrue( 188 | sol['multiplier_equality'] is None 189 | ) 190 | 191 | # new cost 192 | f = np.array([-1.,-.1,0.]) 193 | sol = linear_program(f, A, b) 194 | self.assertAlmostEqual( 195 | sol['min'], 196 | -.55 197 | ) 198 | np.testing.assert_array_almost_equal( 199 | sol['argmin'], 200 | np.array([.5,.5,0.]) 201 | ) 202 | self.assertEqual( 203 | sol['active_set'], 204 | [0,3,8] 205 | ) 206 | np.testing.assert_array_almost_equal( 207 | sol['multiplier_inequality'], 208 | np.array([.05,0.,0.,.05,0.,0.,0.,0.,.9]) 209 | ) 210 | self.assertTrue( 211 | sol['multiplier_equality'] is None 212 | ) 213 | 214 | # new cost 215 | f = np.array([1.,1.,0.]) 216 | sol = linear_program(f, A, b) 217 | self.assertAlmostEqual( 218 | sol['min'], 219 | -1. 220 | ) 221 | self.assertAlmostEqual( 222 | sol['argmin'][0] + sol['argmin'][1], 223 | -1. 224 | ) 225 | self.assertTrue(4 in sol['active_set']) 226 | self.assertTrue(7 in sol['active_set']) 227 | self.assertAlmostEqual(sol['multiplier_inequality'][4], .5) 228 | self.assertAlmostEqual(sol['multiplier_inequality'][7], .5) 229 | self.assertTrue( 230 | sol['multiplier_equality'] is None 231 | ) 232 | 233 | # lower dimensional domain because of the inequalities (in this case one cannot get the active set just looking at the residuals of the inequalities) 234 | f = np.array([1.,0.]) 235 | A = np.array([[1., 0.],[-1., 0.]]) 236 | b = np.zeros(2) 237 | sol = linear_program(f, A, b) 238 | self.assertAlmostEqual( 239 | sol['min'], 240 | 0. 241 | ) 242 | self.assertAlmostEqual( 243 | sol['argmin'][0], 244 | 0. 245 | ) 246 | self.assertEqual( 247 | sol['active_set'], 248 | [1] 249 | ) 250 | np.testing.assert_array_almost_equal( 251 | sol['multiplier_inequality'], 252 | np.array([0.,1.]) 253 | ) 254 | self.assertTrue( 255 | sol['multiplier_equality'] is None 256 | ) 257 | 258 | def test_linear_programming_solver_gurobi(self): 259 | 260 | # use gurobi solver 261 | self.test_linear_programming_solver(linear_program=lp_gurobi) 262 | 263 | def test_quadratic_programming_solver(self, quadratic_program=qp_pnnls): 264 | 265 | # trivial qp with only inequalities 266 | H = np.eye(2) 267 | f = np.ones(2) 268 | A = -np.eye(2) 269 | b = -np.ones(2) 270 | sol = quadratic_program(H, f, A, b) 271 | self.assertAlmostEqual( 272 | sol['min'], 273 | 3. 274 | ) 275 | np.testing.assert_array_almost_equal( 276 | sol['argmin'], 277 | np.array([1.,1.]) 278 | ) 279 | self.assertEqual( 280 | sol['active_set'], 281 | [0,1] 282 | ) 283 | np.testing.assert_array_almost_equal( 284 | sol['multiplier_inequality'], 285 | np.array([2.,2.]) 286 | ) 287 | self.assertTrue( 288 | sol['multiplier_equality'] is None 289 | ) 290 | 291 | # add equality constraints 292 | C = np.array([[0., 1.]]) 293 | d = np.array([2.]) 294 | sol = quadratic_program(H, f, A, b, C, d) 295 | self.assertAlmostEqual( 296 | sol['min'], 297 | 5.5 298 | ) 299 | np.testing.assert_array_almost_equal( 300 | sol['argmin'], 301 | np.array([1.,2.]) 302 | ) 303 | self.assertEqual( 304 | sol['active_set'], 305 | [0] 306 | ) 307 | np.testing.assert_array_almost_equal( 308 | sol['multiplier_inequality'], 309 | np.array([2.,0.]) 310 | ) 311 | np.testing.assert_array_almost_equal( 312 | sol['multiplier_equality'], 313 | np.array([-3.]) 314 | ) 315 | 316 | # unfeasible 317 | sol = quadratic_program(H, f, A, b, C, -d) 318 | for value in sol.values(): 319 | self.assertTrue(value is None) 320 | 321 | # lower dimensional domain because of the inequalities (in this case one cannot get the active set just looking at the residuals of the inequalities) 322 | H = np.eye(2) 323 | f = np.zeros(2) 324 | A = np.array([[1., 0.],[-1., 0.]]) 325 | b = np.array([1.,-1.]) 326 | sol = quadratic_program(H, f, A, b) 327 | self.assertAlmostEqual( 328 | sol['min'], 329 | .5 330 | ) 331 | self.assertAlmostEqual( 332 | sol['argmin'][0], 333 | 1. 334 | ) 335 | self.assertEqual( 336 | sol['active_set'], 337 | [1] 338 | ) 339 | np.testing.assert_array_almost_equal( 340 | sol['multiplier_inequality'], 341 | np.array([0.,1.]) 342 | ) 343 | self.assertTrue( 344 | sol['multiplier_equality'] is None 345 | ) 346 | 347 | def test_quadratic_programming_solver_gurobi(self): 348 | 349 | # use gurobi solver 350 | self.test_quadratic_programming_solver(quadratic_program=qp_gurobi) 351 | 352 | def test_mixed_integer_quadratic_programming_solver_gurobi(self): 353 | 354 | # select solvers 355 | mixed_integer_quadratic_program = miqp_gurobi 356 | 357 | # simple miqp 358 | H = np.eye(2) 359 | f = np.array([0.,-.6]) 360 | nc = 1 361 | A = np.eye(2) 362 | b = 2.*np.ones(2) 363 | sol = mixed_integer_quadratic_program(nc, H, f, A, b) 364 | self.assertAlmostEqual( 365 | sol['min'], 366 | -.1 367 | ) 368 | np.testing.assert_array_almost_equal( 369 | sol['argmin'], 370 | np.array([0.,1.]) 371 | ) 372 | 373 | # add equalities 374 | C = np.array([[1., -1.]]) 375 | d = np.zeros(1) 376 | sol = mixed_integer_quadratic_program(nc, H, f, A, b, C, d) 377 | self.assertAlmostEqual( 378 | sol['min'], 379 | 0. 380 | ) 381 | np.testing.assert_array_almost_equal( 382 | sol['argmin'], 383 | np.zeros(2) 384 | ) 385 | 386 | # unfeasible miqp 387 | C = np.ones((1,2)) 388 | d = 5.*np.ones(1) 389 | sol = mixed_integer_quadratic_program(nc, H, f, A, b, C, d) 390 | self.assertTrue(sol['min'] is None) 391 | self.assertTrue(sol['argmin'] is None) 392 | 393 | if __name__ == '__main__': 394 | unittest.main() -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | scipy<=1.11.4 2 | numpy 3 | sympy 4 | matplotlib 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | setup(name='pympc', 4 | version='0.1', 5 | description='Toolbox for Model Predictive Control of linear and hybrid systems', 6 | url='https://github.com/TobiaMarcucci/pympc', 7 | author='Tobia Marcucci', 8 | author_email='tobiam@mit.edu', 9 | license='MIT', 10 | packages=find_packages(), 11 | keywords=[ 12 | 'model predictive control', 13 | 'computational geometry' 14 | ], 15 | install_requires=[ 16 | 'six', 17 | 'numpy', 18 | 'scipy<=1.11.4', 19 | 'matplotlib', 20 | 'gurobipy' 21 | ], 22 | zip_safe=False) 23 | --------------------------------------------------------------------------------