remove polytope dependence; implement MPC iosys w/ notebook example

murrayrm · murrayrm · commit bd322e7befd8 · 2021-02-26T22:46:34.000-08:00
diff --git a/control/obc.py b/control/obc.py
@@ -103,9 +103,8 @@ def __init__(
         # Initial guess
         #
         # We store an initial guess (zero input) in case it is not specified
-        # later.
-        #
-        # TODO: add the ability to overwride this when calling the optimizer.
+        # later.  Note that create_mpc_iosystem() will reset the initial guess
+        # based on the current state of the MPC controller.
         #
         self.initial_guess = np.zeros(
             self.system.ninputs * self.time_vector.size)
@@ -128,24 +127,25 @@ def cost_function(self, inputs):
         x = self.x
         inputs = inputs.reshape(
             (self.system.ninputs, self.time_vector.size))
-        
+
         # Simulate the system to get the state
         _, _, states = ct.input_output_response(
             self.system, self.time_vector, inputs, x, return_x=True)
-        
+
         # Trajectory cost
         # TODO: vectorize
         cost = 0
         for i, time in enumerate(self.time_vector):
-            cost += self.integral_cost(states[:,i], inputs[:,i]) 
-            
+            cost += self.integral_cost(states[:,i], inputs[:,i])
+
         # Terminal cost
         if self.terminal_cost is not None:
             cost += self.terminal_cost(states[:,-1], inputs[:,-1])
-            
+
         # Return the total cost for this input sequence
         return cost
 
+
     #
     # Constraints
     #
@@ -188,7 +188,7 @@ def constraint_function(self, inputs):
         x = self.x
         inputs = inputs.reshape(
             (self.system.ninputs, self.time_vector.size))
-            
+
         # Simulate the system to get the state
         _, _, states = ct.input_output_response(
             self.system, self.time_vector, inputs, x, return_x=True)
@@ -234,15 +234,15 @@ def __call__(self, x, squeeze=None):
     def mpc(self, x, squeeze=None):
         """Compute the optimal input at state x"""
         _, inputs = self.compute_trajectory(x, squeeze=squeeze)
-        return None if inputs is None else inputs.transpose()[0]
+        return None if inputs is None else inputs[:,0]
 
     # Compute the optimal trajectory from the current state
     def compute_trajectory(
             self, x, squeeze=None, transpose=None, return_x=None):
         """Compute the optimal input at state x"""
         # Store the initial state (for use in constraint_function)
         self.x = x
-        
+
         # Call ScipPy optimizer
         res = sp.optimize.minimize(
             self.cost_function, self.initial_guess,
@@ -263,14 +263,33 @@ def compute_trajectory(
                 self.system, self.time_vector, inputs, x, return_x=True)
         else:
             states=None
-            
+
         return _process_time_response(
             self.system, self.time_vector, inputs, states,
             transpose=transpose, return_x=return_x, squeeze=squeeze)
 
+    # Create an input/output system implementing an MPC controller
+    def create_mpc_iosystem(self, dt=True):
+        def _update(t, x, u, params={}):
+            inputs = x.reshape((self.system.ninputs, self.time_vector.size))
+            self.initial_guess = np.hstack(
+                [inputs[:,1:], inputs[:,-1:]]).reshape(-1)
+            _, inputs = self.compute_trajectory(u)
+            return inputs.reshape(-1)
+
+        def _output(t, x, u, params={}):
+            inputs = x.reshape((self.system.ninputs, self.time_vector.size))
+            return inputs[:,0]
+
+        return ct.NonlinearIOSystem(
+            _update, _output, dt=dt,
+            inputs=self.system.nstates,
+            outputs=self.system.ninputs,
+            states=self.system.ninputs * self.time_vector.size)
+
 
 #
-# Create a polytope constraint on the system state
+# Create a polytope constraint on the system state: A x <= b
 #
 # As in the cost function evaluation, the main "trick" in creating a constrain
 # on the state or input is to properly evaluate the constraint on the stacked
@@ -283,26 +302,48 @@ def compute_trajectory(
 # optimization methods LinearConstraint and NonlinearConstraint as "types" to
 # keep things consistent with the terminology in scipy.optimize.
 #
-def state_poly_constraint(sys, polytope):
+def state_poly_constraint(sys, A, b):
+    """Create state constraint from polytope"""
+    # TODO: make sure the system and constraints are compatible
+
+    # Return a linear constraint object based on the polynomial
+    return (opt.LinearConstraint,
+            np.hstack([A, np.zeros((A.shape[0], sys.ninputs))]),
+            np.full(A.shape[0], -np.inf), polytope.b)
+
+
+def state_range_constraint(sys, lb, ub):
     """Create state constraint from polytope"""
     # TODO: make sure the system and constraints are compatible
 
     # Return a linear constraint object based on the polynomial
     return (opt.LinearConstraint,
             np.hstack(
-                [polytope.A, np.zeros((polytope.A.shape[0], sys.ninputs))]),
-            np.full(polytope.A.shape[0], -np.inf), polytope.b)
+                [np.eye(sys.nstates), np.zeros((sys.nstates, sys.ninputs))]),
+            np.array(lb), np.array(ub))
+
 
 # Create a constraint polytope on the system input
-def input_poly_constraint(sys, polytope):
+def input_poly_constraint(sys, A, b):
+    """Create input constraint from polytope"""
+    # TODO: make sure the system and constraints are compatible
+
+    # Return a linear constraint object based on the polynomial
+    return (opt.LinearConstraint,
+            np.hstack(
+                [np.zeros((A.shape[0], sys.nstates)), A]),
+            np.full(A.shape[0], -np.inf), b)
+
+
+def input_range_constraint(sys, lb, ub):
     """Create input constraint from polytope"""
     # TODO: make sure the system and constraints are compatible
 
     # Return a linear constraint object based on the polynomial
     return (opt.LinearConstraint,
             np.hstack(
-                [np.zeros((polytope.A.shape[0], sys.nstates)), polytope.A]),
-            np.full(polytope.A.shape[0], -np.inf), polytope.b)
+                [np.zeros((sys.ninputs, sys.nstates)), np.eye(sys.ninputs)]),
+            np.array(lb), np.array(ub))
 
 
 #
@@ -316,9 +357,9 @@ def input_poly_constraint(sys, polytope):
 # imposing a linear constraint:
 #
 #     np.hstack(
-#         [polytope.A @ sys.C, np.zeros((polytope.A.shape[0], sys.ninputs))])
+#         [A @ sys.C, np.zeros((A.shape[0], sys.ninputs))])
 #
-def output_poly_constraint(sys, polytope):
+def output_poly_constraint(sys, A, b):
     """Create output constraint from polytope"""
     # TODO: make sure the system and constraints are compatible
 
@@ -329,12 +370,12 @@ def _evaluate_output_constraint(x):
         states = x[:sys.nstates]
         inputs = x[sys.nstates:]
         outputs = sys._out(0, states, inputs)
-        return polytope.A @ outputs
+        return A @ outputs
 
     # Return a nonlinear constraint object based on the polynomial
     return (opt.NonlinearConstraint,
             _evaluate_output_constraint,
-            np.full(polytope.A.shape[0], -np.inf), polytope.b)
+            np.full(A.shape[0], -np.inf), b)
 
 
 #
@@ -345,8 +386,8 @@ def _evaluate_output_constraint(x):
 # evaluates to associted quadratic cost.  This is compatible with the way that
 # the `cost_function` evaluates the cost at each point in the trajectory.
 #
-def quadratic_cost(sys, Q, R):
+def quadratic_cost(sys, Q, R, x0=0, u0=0):
     """Create quadratic cost function"""
     Q = np.atleast_2d(Q)
     R = np.atleast_2d(R)
-    return lambda x, u: x @ Q @ x + u @ R @ u
+    return lambda x, u: ((x-x0) @ Q @ (x-x0) + (u-u0) @ R @ (u-u0)).item()
diff --git a/control/tests/obc_test.py b/control/tests/obc_test.py
@@ -10,7 +10,7 @@
 import scipy as sp
 import control as ct
 import control.obc as obc
-import polytope as pc
+from control.tests.conftest import slycotonly
 
 def test_finite_horizon_mpc_simple():
     # Define a linear system with constraints
@@ -21,8 +21,7 @@ def test_finite_horizon_mpc_simple():
 
     # State and input constraints
     constraints = [
-        obc.state_poly_constraint(sys, pc.box2poly([[-5, 5], [-5, 5]])),
-        obc.input_poly_constraint(sys, pc.box2poly([[-1, 1]])),
+        (sp.optimize.LinearConstraint, np.eye(3), [-5, -5, -1], [5, 5, 1]),
     ]
 
     # Quadratic state and input penalty
@@ -48,10 +47,12 @@ def test_finite_horizon_mpc_simple():
     # Convert controller to an explicit form (not implemented yet)
     # mpc_explicit = obc.explicit_mpc();
 
-    # Test explicit controller 
+    # Test explicit controller
     # u_explicit = mpc_explicit(x0)
     # np.testing.assert_array_almost_equal(u_openloop, u_explicit)
 
+
+@slycotonly
 def test_finite_horizon_mpc_oscillator():
     # oscillator model defined in 2D
     # Source: https://www.mpt3.org/UI/RegulationProblem
@@ -65,8 +66,7 @@ def test_finite_horizon_mpc_oscillator():
 
     # state and input constraints
     trajectory_constraints = [
-        obc.output_poly_constraint(sys, pc.box2poly([[-10, 10]])),
-        obc.input_poly_constraint(sys, pc.box2poly([[-1, 1]]))
+        (sp.optimize.LinearConstraint, np.eye(3), [-10, -10, -1], [10, 10, 1]),
     ]
 
     # Include weights on states/inputs
@@ -85,3 +85,57 @@ def test_finite_horizon_mpc_oscillator():
 
     # Add tests to make sure everything works
     t, u_openloop = optctrl.compute_trajectory([1, 1])
+
+
+def test_mpc_iosystem():
+    # model of an aircraft discretized with 0.2s sampling time
+    # Source: https://www.mpt3.org/UI/RegulationProblem
+    A = [[0.99, 0.01, 0.18, -0.09,   0],
+         [   0, 0.94,    0,  0.29,   0],
+         [   0, 0.14, 0.81,  -0.9,   0],
+         [   0, -0.2,    0,  0.95,   0],
+         [   0, 0.09,    0,     0, 0.9]]
+    B = [[ 0.01, -0.02],
+         [-0.14,     0],
+         [ 0.05,  -0.2],
+         [ 0.02,     0],
+         [-0.01, 0]]
+    C = [[0, 1, 0, 0, -1],
+         [0, 0, 1, 0,  0],
+         [0, 0, 0, 1,  0],
+         [1, 0, 0, 0,  0]]
+    model = ct.ss2io(ct.ss(A, B, C, 0, 0.2))
+
+    # For the simulation we need the full state output
+    sys = ct.ss2io(ct.ss(A, B, np.eye(5), 0, 0.2))
+
+    # compute the steady state values for a particular value of the input
+    ud = np.array([0.8, -0.3])
+    xd = np.linalg.inv(np.eye(5) - A) @ B @ ud
+    yd = C @ xd
+
+    # provide constraints on the system signals
+    constraints = [obc.input_range_constraint(sys, [-5, -6], [5, 6])]
+
+    # provide penalties on the system signals
+    Q = model.C.transpose() @ np.diag([10, 10, 10, 10]) @ model.C
+    R = np.diag([3, 2])
+    cost = obc.quadratic_cost(model, Q, R, x0=xd, u0=ud)
+
+    # online MPC controller object is constructed with a horizon 6
+    optctrl = obc.OptimalControlProblem(
+        model, np.arange(0, 6) * 0.2, cost, constraints)
+
+    # Define an I/O system implementing model predictive control
+    ctrl = optctrl.create_mpc_iosystem()
+    loop = ct.feedback(sys, ctrl, 1)
+
+    # Choose a nearby initial condition to speed up computation
+    X0 = np.hstack([xd, np.kron(ud, np.ones(6))]) * 0.99
+
+    Nsim = 10
+    tout, xout = ct.input_output_response(
+        loop, np.arange(0, Nsim) * 0.2, 0, X0)
+
+    # Make sure the system converged to the desired state
+    np.testing.assert_almost_equal(xout[0:sys.nstates, -1], xd, decimal=1)
diff --git a/examples/mpc_aircraft.ipynb b/examples/mpc_aircraft.ipynb
