# Example 9: Robust Controlled Invariance or Constrained Quadratic Regulator

The following theorem encodes the conditions for robust controlled invariance using zonotope containment. As a byproduct, we also obtain polytopic control laws and polytopic Lyapunov functions for linear discrete-time systems. 
### Theorem
Consider a discrete-time disturbed linear system 
\begin{equation}
    x^+ \in Ax + Bu \oplus \mathbb{W},
\end{equation}
where $x \in \mathbb{R}^n$ is the state, $u \in \mathbb{R}^{m}$ is the control input, and $\mathbb{W}=\langle W \rangle \in \mathbb{R}^n$,  is the zonotope disturbance and $W \in \mathbb{R}^{n \times n_w}$. If there exist $\phi \in \mathbb{R}^{n \times q}, \theta \in \mathbb{R}^{m \times q}$, where $q$ is user-defined, such that the following properties hold:
$$
\exists E \in \mathbb{R}^{n \times n_w}, \exists \alpha \in [0,1), 
$$

$$
(A \phi + B \theta, W) = (E,\phi),~~ \langle  E \rangle \subseteq \alpha
 \langle  W \rangle.
$$
$$
    \mathcal{X} := \frac{1}{1-\alpha} \langle \phi \rangle, \mathcal{U} := \frac{1}{1-\alpha} \langle \theta \rangle.
$$
Then, $\mathcal{X}$ is a robust control invariant set under the control law $\pi: \mathcal{X} \rightarrow \mathcal{U}$, where $ \mathcal{X} := \frac{1}{1-\alpha} \langle \phi \rangle, \mathcal{U} := \frac{1}{1-\alpha} \langle \theta \rangle$ and
$$
    \pi(x)=\theta~ \underset{\zeta, x=\phi \zeta}{\arg \min}~ \| \zeta \|_\infty. \\
$$
Moroever, $V : \mathbb{R}^n \rightarrow \mathbb{R}_+$ serves as a Lyapunov function for the undisturbed closed-loop system $Ax+B\pi(x)$, where:
$$
    \begin{array}{lll}
    V(x)=& \min & \| \zeta \|_\infty \\
    & \text{subject to} & x= \phi \zeta.
    \end{array}
$$

### Proof
Given $x= \frac{1}{1-\alpha} \phi \zeta, \|\zeta\|_\infty \le 1$, the control policy chooses $u=\frac{1}{1-\alpha} \theta \zeta$. Therefore, $x^+ \in \frac{1}{1-\alpha} \langle A\phi+B\theta \rangle \oplus \mathbb{W}$. We need to show that 
$$
    \frac{1}{1-\alpha} \langle A\phi+B\theta \rangle \oplus \mathbb{W} \in \frac{1}{1-\alpha} \langle \phi \rangle.
$$
From above we have the following:
$$
\label{eq_rci_proof_main}
    \langle A\phi + B \theta \rangle \oplus \mathbb{W} \subseteq \alpha \mathbb{W} \oplus \langle  \phi \rangle 
$$
In order to derive the proof, we divide both sides by $(1-\alpha)$ and then take  $\frac{\alpha}{1-\alpha}\mathbb{W}$ out from both sides. Note that this is a valid operation by the virtue of properties of support functions. A quick inspection also reveals that the Lyapunov function level sets are scalar multiplications of $\mathcal{X}$ and we have $V(A x + B\pi(x))  \subseteq   V(x) \langle \phi \rangle$.

Theorem above provides a framework to compute constrained robust control invariant sets via subset encodings.  Note that $\pi(x)$ is given as a linear program hence its explicit form is, in general, piecewise linear \cite{bemporad2000piecewise}. Given polyhedral constraints $\mathbb{X}$ and $\mathbb{U}$ on state and control, respectively, we can compute the constrained Quadratic Regulator (QR) with the following quadratic program:
\begin{equation}
\label{eq_quadratic_rci}
        \begin{array}{ll}
    \min &  \text{tr}{(\phi \phi' Q)} + \text{tr}{(\theta \theta' R)} \\
     \text{subject to} & (A \phi + B \theta, W) = (E,\phi),\\
    &  \langle \phi \rangle  \subseteq (1-\alpha) \mathbb{X}, \langle \theta \rangle  \subseteq (1-\alpha) \mathbb{U},
    \\
    &\langle E \rangle \subseteq \alpha \langle W \rangle, \\
    & 0 \le \alpha \le 1,
    \end{array}
\end{equation}
where $Q,R$ are appropriately sized positive definite matrix penalizing the zonotope generators of states and controls, respectively.   

### Example:
Consider a system where $A=I+A_c\delta, B=[\frac{1}{2}\delta^2,\delta]$, where $A_c=[(0,1),(0,1)]$ and $\delta=0.01$, representing a double integrator with negative damping. The constraint sets are $\mathbb{X}=\mathbb{B}_1$ and $\mathbb{U}=20\mathbb{B}_\infty$, and $W=([(1,-1),(1,1)])\times \frac{\delta}{4}$. We solved with $Q=I, R=100 $. The smallest $q$ that makes the program feasible is $13$ - we deliberately selected a   disturbance set such that no linear feedback policy $u=Kx$ for any $K$ solves this problem. The sets and sample undisturbed trajectories are shown.


```python
import numpy as np
import pypolycontain as pp
import pydrake.solvers.mathematicalprogram as MP
import pydrake.solvers.gurobi as Gurobi_drake
# use Gurobi solver
global gurobi_solver, license
gurobi_solver=Gurobi_drake.GurobiSolver()
license = gurobi_solver.AcquireLicense()
np.random.seed(0)
```


```python
n=2
A=np.eye(n)+np.random.randint(-5,5,size=(n,n))*1/10
A=np.eye(n)+np.random.randint(-5,5,size=(n,n))*1/10

dt=0.01
A=np.array([[1,dt],[0,1+1*dt]])
B=np.array([[dt**2/2,dt]]).reshape(2,1)
W=pp.zonotope(x=np.zeros((n,1)),G=np.array([[1,-1],[1,1]])*0.25*dt)
# X=pp.zonotope(x=np.zeros((n,1)),G=np.array([[1,0],[-0.5,1]]) ,color='cyan')
H=np.array([[-1,1],\
           [1,-1],\
           [-1,-1],[1,1]]).reshape(4,2)
h=np.array([1,1,1,1]).reshape(4,1)
X=pp.H_polytope(H,h,color='blue')
U=pp.zonotope(x=np.zeros((1,1)),G=np.eye(1)*20)
# Cost
Q=np.eye(n)
R=np.eye(1)*10000
pp.visualize([X])
```


```python
prog=MP.MathematicalProgram()
n,m=A.shape[0],B.shape[1]
q=13
program = MP.MathematicalProgram()
phi=program.NewContinuousVariables(n,q,'phi')
theta=program.NewContinuousVariables(m,q,'theta')
alpha=program.NewContinuousVariables(1,'alpha')
program.AddBoundingBoxConstraint(0,1,alpha)
program.AddQuadraticCost(np.trace( np.linalg.multi_dot([phi.T,Q,phi])   ))
program.AddQuadraticCost(np.trace( np.linalg.multi_dot([theta.T,R,theta])   ))
K=np.hstack(( (np.dot(A,phi) + np.dot(B,theta))[:,n:] , W.G ))
program.AddLinearConstraint (  np.equal(K, phi, dtype='object').flatten() )
inbody=pp.zonotope(x=np.zeros((n,1)), G=(np.dot(A,phi)+np.dot(B,theta))[:,0:n])
_W=pp.to_AH_polytope(W)
_W.P.h=_W.P.h*alpha
_X=pp.to_AH_polytope(X)
_X.P.h=_X.P.h*(1-alpha)
_U=pp.to_AH_polytope(U)
_U.P.h=_U.P.h*(1-alpha)
pp.subset(program, inbody,circumbody=_W)
pp.subset(program, pp.zonotope(x=np.zeros((2,1)),G=phi),circumbody=_X)
pp.subset(program, pp.zonotope(x=np.zeros((1,1)),G=theta),circumbody=_U)
result=gurobi_solver.Solve(program,None,None)
if result.is_success():
    print("sucsess")
    alpha_n=result.GetSolution(alpha)
    phi_n= result.GetSolution(phi)
    theta_n= result.GetSolution(theta)
    Omega=pp.zonotope(x=np.zeros((2,1)),G=phi_n/(1-alpha_n),color='red')
    pp.visualize([X,Omega],title='Robust control invariant set $\mathcal{X}$ (red) \n \
    inside safe set $\mathbb{X}$ (blue)',figsize=(6,6),a=0.02)
    print("alpha was",alpha_n[0])
else:
    print("failure")
```


```python
def control(phi,theta,x):
    program = MP.MathematicalProgram()
    n,q=phi.shape
    assert n,q==theta.shape
    zeta=program.NewContinuousVariables(q,1,'zeta')
    zeta_inf=program.NewContinuousVariables(1,1,'zeta_phi')
    program.AddLinearConstraint( np.equal(x, np.dot(phi,zeta), dtype='object').flatten() )
    program.AddLinearConstraint( np.greater_equal( zeta_inf, zeta, dtype='object').flatten() )  
    program.AddLinearConstraint( np.greater_equal( zeta_inf, -zeta, dtype='object').flatten() )
    program.AddLinearCost( np.array([1]),np.array([0]), zeta_inf )   
    result=gurobi_solver.Solve(program,None,None)
    if result.is_success():
#         print("sucsess")
        zeta_n=result.GetSolution(zeta)
#         print(  result.GetSolution(zeta_inf)[0] )
        return np.dot(theta,zeta_n).reshape(1,1),result.GetSolution(zeta_inf)[0]

    
import matplotlib.pyplot as plt

fig,ax=plt.subplots()
# pp.visualize([pp.zonotope(G=V[t]*phi_n,color=(1-0.8*V[t]/V[0],0,0)) \
#               for t in range(T)],ax=ax,fig=fig,\
#              title=r'Sample Trajectory and Lyapunov Function Level Sets',a=0.1)
pp.visualize([pp.zonotope(G=p/15*phi_n/(1-alpha_n),color=(1*p/15,0,0)) \
              for p in range(15,1,-1)],ax=ax,fig=fig,\
             title='Sample Undisturbed Trajectories and \n Lyapunov Function Level Sets',a=0.02)
x0=pp.vcube(q)
fig.set_size_inches(6,6)
N=101
for i in range(N):
    x={}
    u={}
    V={}
    T=40
    zeta=2*(np.random.random(size=(q,1))-0.5)
    zeta=x0[int(i*len(x0)/N),:].reshape(q,1)
    x[0]=np.dot(phi_n/(1-alpha_n),  zeta/max(np.abs(zeta)))
    for t in range(T):
        u[t],V[t]=control(phi_n,theta_n,x[t])
        x[t+1]=np.dot(A,x[t])+np.dot(B,u[t])
#     ax.plot([x[t][0,0] for t in range(T+1)],[x[t][1,0] for t in range(T+1)],'*',color='cyan')
    ax.plot([x[t][0,0] for t in range(1)],[x[t][1,0] for t in range(1)],'*',color='black',MarkerSize=1)
    ax.plot([x[t][0,0] for t in range(T+1)],[x[t][1,0] for t in range(T+1)],'--',color='black',Linewidth=1)
```


```python
K=np.dot( theta_n, np.linalg.pinv(phi_n)  )
```


```python
A_cl=A+B@K
np.linalg.eigvals(A_cl)
```




    array([0.98768713, 0.90900118])




```python
K@phi_n-theta_n
```




    array([[ 0.19712037, -0.19475627,  0.09975514, -0.10199905,  0.01056548,
            -0.02595605, -0.04187009, -0.02414759, -0.06270881,  0.05449374,
            -0.13783213,  0.1248632 , -0.20471651]])




```python

```

30f1077ce8ec76d5b751281138fd8a30792884b2
ipynb
Jupyter Notebook
doc/source/Example 9.ipynb
sadraddini/pypolycontain
807846634899fa3de543fe09178ff7bda4386541
AFL-1.1
2019-01-09T20:09:27.000Z
2021-12-02T15:16:40.000Z
doc/source/Example 9.ipynb
sadraddini/pypolycontain
807846634899fa3de543fe09178ff7bda4386541
AFL-1.1
2019-03-16T01:56:35.000Z
2020-07-31T20:32:06.000Z
doc/source/Example 9.ipynb
sadraddini/pypolycontain
807846634899fa3de543fe09178ff7bda4386541
AFL-1.1
2020-07-30T23:16:11.000Z
2020-07-30T23:16:11.000Z
Qwen/Qwen-72B
1. YES
2. YES