import numpy as np
from typing import Optional, Tuple
import control as ct
import warnings
from mpt4py.base import Matrix, Vector, HData
from mpt4py.systems.system_base import SystemBase
from mpt4py.geometry.polyhedron import Polyhedron
from mpt4py.util import is_positive_semidefinite
from mpt4py.exceptions import InfeasibleError
[docs]
class LTISystem(ct.StateSpace, SystemBase):
r"""
Linear Time Invariant (LTI) System.
This is a wrapper of the LTI class in Python Control Library.
"""
def __init__(self, *args, **kwargs) -> None:
super().__init__(*args, **kwargs)
self._state_con = None
self._input_con = None
self._terminal_con = None
self._Q = kwargs.get("Q")
self._R = kwargs.get("R")
self._Qf = kwargs.get("Qf")
self._lbx = kwargs.get("lbx")
self._ubx = kwargs.get("ubx")
self._lbu = kwargs.get("lbu")
self._ubu = kwargs.get("ubu")
self._x_ref = kwargs.get('x_ref')
self._u_ref = kwargs.get('u_ref')
if self._x_ref is None:
self._x_ref = np.zeros(self.nx)
if self._u_ref is None:
self._u_ref = np.zeros(self.nu)
[docs]
def discretize(self, Ts: float, *args, **kwargs) -> "LTISystem":
if self.isdtime():
raise ValueError("Discretization can only be performed on continuous-time systems.")
if Ts <= 0:
raise ValueError("The sampling time must be positive.")
return self.sample(Ts, *args, **kwargs) # TODO: this is problematic because self.sample() returns a StateSpace object but not LTISystem object
@property
def Q(self) -> Matrix:
return self._Q
@Q.setter
def Q(self, Q: Matrix):
if Q.shape != (self.nx, self.nx):
raise ValueError("Q must be a matrix of shape ({}, {})".format(self.nx, self.nx))
if not is_positive_semidefinite(Q):
raise ValueError("Q must be a positive semi-definite matrix.")
self._Q = Q
@property
def R(self) -> Matrix:
return self._R
@R.setter
def R(self, R: Matrix):
if R.shape != (self.nu, self.nu):
raise ValueError("R must be a matrix of shape ({}, {})".format(self.nu, self.nu))
if not is_positive_semidefinite(R):
raise ValueError("R must be a positive semi-definite matrix.")
self._R = R
@property
def Qf(self) -> Matrix:
return self._Qf
@Qf.setter
def Qf(self, Qf: Matrix):
if Qf.shape != (self.nx, self.nx):
raise ValueError("Qf must be a matrix of shape ({}, {})".format(self.nx, self.nx))
if not is_positive_semidefinite(Qf):
raise ValueError("Qf must be a positive semi-definite matrix.")
self._Qf = Qf
@property
def x_ref(self) -> Vector:
return self._x_ref
@x_ref.setter
def x_ref(self, x_ref: Vector):
if x_ref.shape != (self.nx, ):
raise ValueError("x_ref must be a vector of shape ({},)".format(self.nx, ))
self._x_ref = x_ref
@property
def u_ref(self) -> Vector:
return self._u_ref
@u_ref.setter
def u_ref(self, u_ref: Vector):
if u_ref.shape != (self.nu, ):
raise ValueError("u_ref must be a vector of shape ({},)".format(self.nu))
self._u_ref = u_ref
@property
def state_constraints(self) -> Polyhedron:
return self._state_con
@state_constraints.setter
def state_constraints(self, state_set: Polyhedron):
if not isinstance(state_set, Polyhedron):
raise TypeError('State constraints must be a Polyhedron.')
# TODO: shall we require X to be non-empty or full-dimensional?
if state_set.is_empty:
warnings.warn('The state constraints results in an empty set.')
self._state_con = state_set
@property
def input_constraints(self) -> Polyhedron:
return self._input_con
@input_constraints.setter
def input_constraints(self, input_set: Polyhedron):
if not isinstance(input_set, Polyhedron):
raise TypeError('Input constraints must be a Polyhedron.')
if input_set.is_empty:
warnings.warn('The input constraints results in an empty set.')
self._input_con = input_set
@property
def terminal_constraints(self) -> Polyhedron:
return self._terminal_con
@terminal_constraints.setter
def terminal_constraints(self, terminal_set: Polyhedron):
if not isinstance(terminal_set, Polyhedron):
raise TypeError('Terminal constraints must be a Polyhedron.')
if terminal_set.is_empty:
warnings.warn('The terminal constraints results in an empty set.')
self._terminal_con = terminal_set
# @property
# def lbx(self) -> Vector:
# return self._lbx
# @lbx.setter
# def lbx(self, lbx: Vector):
# self._lbx = lbx
# @property
# def ubx(self) -> Vector:
# return self._ubx
# @ubx.setter
# def ubx(self, ubx: Vector):
# self._ubx = ubx
# @property
# def lbu(self) -> Vector:
# return self._lbu
# @lbu.setter
# def lbu(self, lbu: Vector, index: Optional[Tuple[int, list[int]]] = None):
# self._lbu = lbu
[docs]
def invariant_set(self, K: Matrix, max_itr: Optional[int] = 100, verbose: Optional[bool] = False) -> Tuple[Polyhedron, bool]:
r"""
Compute the (maximal) invariant set using the given terminal controller :math:`u=Kx`.
"""
# TODO: this function only deals with controlled linear systems. Need to consider autonomous systems.
if not self.isdtime(strict=True):
raise NotImplementedError('The invariant set must be computed only for strictly discrete time systems.')
if K.shape != (self.nu, self.nx):
raise ValueError("K must be a matrix of shape ({}, {})".format(self.nu, self.nx))
if max_itr <= 0:
raise ValueError("The maximum number of iterations should be a positive integer.")
# TODO: deal with this case
if self._state_con is None:
raise NotImplementedError('The Polyhedron class cannot represent the whole space yet.')
if self.input_constraints is not None:
if self.input_constraints.is_empty:
raise InfeasibleError('The input constraints must not be an empty set.')
if self.state_constraints is not None:
if self.state_constraints.is_empty:
raise InfeasibleError('The state constraints must not be an empty set.')
A_cl = self.A + self.B @ K # closed-loop system dynamics x^+ = A_cl x
if self.input_constraints is None:
O = self.state_constraints
else:
O = self.state_constraints.intersect(Polyhedron(HData(self.input_constraints.A @ K, self.input_constraints.b)))
O.minHrep()
itr = 0
converged = False
while itr < max_itr:
Oprev = O
preO = Polyhedron(HData(O.A @ A_cl, O.b)) # compute the pre-set
O = O.intersect(preO)
O.minHrep(True)
_ = O.Vrep
if O == Oprev:
converged = True
break
if verbose:
print("Iteration {0}... not yet equal\n".format(itr))
itr += 1
if verbose:
if converged:
print('Maximal invariant set computed after ' + str(itr) + ' iterations\n\n')
else:
print('Maximal invariant set did not converge after ' + str(itr) + ' iterations\n\n')
return O, converged
[docs]
def control_invariant_set(self):
"""
Compute the control invariant set of the given MPC controller.
"""
raise NotImplementedError
@property
def lqr_gain(self) -> Matrix:
r"""
Compute the infinite-horizon LQR gain :math:`u=Kx`.
"""
if self.Q is None:
raise ValueError("Q must be specified to compute the LQR gain.")
if self.R is None:
raise ValueError("R must be specified to compute the LQR gain.")
K, _, _ = ct.dlqr(self.A, self.B, self.Q, self.R)
return -K
@property
def lqr_set(self, max_itr: Optional[int] = 100) -> Tuple[Polyhedron, bool]:
"""
Compute the (maximal) invariant set using infinite-horizon LQR as the terminal controller.
"""
return self.invariant_set(self.lqr_gain, max_itr)