# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: percent
#       format_version: '1.3'
#       jupytext_version: 1.13.6
#   kernelspec:
#     display_name: Python 3 (ipykernel)
#     language: python
#     name: python3
# ---

# %% [markdown]
# ---
# # <center><font color=purple>[python-control](https://python-control.readthedocs.io/en/0.8.3/)</font> </center>
# ---
#
# + [GitHub](https://github.com/python-control/python-control)
# + [Murray](https://www.cds.caltech.edu/~murray/wiki/Control_Systems_Library_for_Python)
# + [Example](https://www.cds.caltech.edu/~murray/wiki/index.php?title=Python-control/Example:_Vertical_takeoff_and_landing_aircraft)
# + [pdf](http://robertobucher.dti.supsi.ch/wp-content/uploads/2017/03/BookPythonForControl.pdf)

# %% [markdown]
# # Standard import
#
# There are two different ways to use the package. For the default interface described in Function reference, simply import the control package as follows:

# %%
import control as co

# %% [markdown]
# ---
# # <center> <font color=darkgreen>Dynamical systems</font>  </center>
#
# ---
#
# Recall the system studied in the *machine learning* problem.
# + It was *static*, in the sense that the outputs depended only on the inputs and not on an internal state. 
# + It was *black box*, meaning that we represented the output as a probability distribution conditioned on the input. 
#
# In the study of dynamical systems we make a few changes. 
# + The system is **dynamic**. The outputs at time $t$ depend, not only on the inputs at time $t$, but also on the **state of the system** at time $t$. 
# + We have a deterministic model of the system. We are not building the model based on data (as in ML), but rather based on physical principles. 
#
# There is also a change of notation.
# + $u(t)$: system input (in ML it was $x$).
# + $x(t)$: system state.
#
# The figure below depicts a **linear** dynamical system.
# <img src="../figs/lti.svg" width="400">
#
#
# ---
# # <center> <font color=darkgreen>Systems representations</font>  </center>
#
# ---
#
# Linear dynamical systems can be **continuous time** or **discrete time**. The theory for these two is similar (with caveats) and is taught in **ME 132**.
#
# **Note**: We will focus this tutorial on continuous-time systems. 
#
# Also, there are two levels of *granularity* of models: **state-space** models and **input-output** models.
# + input-output models predict the **output** of the system given the input. Their mathematical form is the **transfer function**.
# + state-space models predict the **output** and the **state** of the system, given the input. Their mathematical form is the **state and output equations**.
#
# <img src="../figs/modeltypes.svg" width="500">
#
# ---
#

# %%
import matplotlib.pyplot as plt
import numpy as np

def plotresponse(t,y):
    plt.figure(figsize=(10,4))
    plt.plot(t,y,linewidth=3)
    plt.xlabel('t',fontsize=14)
    plt.ylabel('y',fontsize=14)
    plt.grid()
    
def plotstepresponse(sys):
    
    t, y = co.step_response(sys)
    r = np.ones(t.shape)

    plt.figure(figsize=(10,4))
    plt.plot(t,y,t,r,linewidth=3)
    plt.xlabel('t',fontsize=14)
    plt.legend(['y','r'],fontsize=14)
    plt.grid()


# %% [markdown]
# ---
# # <center> <font color=darkgreen>Systems creation</font>  </center>
#
# ---
#
# <img src="../figs/massspringdamperforce.svg" width="250">
#
# \begin{align}
# \frac{d}{dt}\begin{bmatrix}x\\ \dot{x}\end{bmatrix} &= 
# \begin{bmatrix} 0&1\\-\frac{k}{m}&-\frac{b}{m} \end{bmatrix}\begin{bmatrix}x\\ \dot{x}\end{bmatrix} + 
# \begin{bmatrix} 0 \\ 1/m \end{bmatrix} F & \text{state equation} \\
# x &= \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} x \\ \dot{x} \end{bmatrix}  & \text{output equation} 
# \end{align}
#
#
# ##  State-space systems `ss(A,B,C,D)`

# %%
m = 1
k = 1
b = 2
A = [[0,1],[-k/m,-b/m]]
B = [[0],[1/m]]
C = [1,0]
D = [0]
sys = co.ss(A,B,C,D)

print(sys)

# %% [markdown]
# ## Input-output systems `tf(num, den)`
#
# \begin{equation}
# G(s) = \frac{X(s)}{F(s)} =   \frac{1}{m s^2 + b s + k }
# \end{equation}

# %%
G = co.tf(1,[m,b,k])

print(G)

# %% [markdown]
# **Note**: It is a common technique to define a *unit transfer function* `tf('s')`, and use it to compose other transfer functions, as follows:

# %%
s = co.tf('s')
G  = (s + 1)/(s**2 + 5*s + 6)
print(G)

# %% [markdown]
# ---
# # <center> <font color=darkgreen>Systems connections</font>  </center>
#
# ---

# %% [markdown]
# ## [`series(sys1,sys2)`](https://python-control.readthedocs.io/en/v0.6d/bdalg_strings.html#control.series)
#
# <img src="../figs/conn_series.svg" width="300">
#

# %%
Gstiff = co.tf(1,[1,2,100])
Gsoft  = co.tf(1,[1,2,3])

Gseries = co.series(Gstiff,Gsoft)

t, y = co.impulse_response(Gseries)
plotresponse(t,y)

# %% [markdown]
# ## [`parallel(sys1,sys2)`](https://python-control.readthedocs.io/en/v0.6d/bdalg_strings.html#control.parallel)
#
# <img src="../figs/conn_parallel.svg" width="300">
#

# %%
Gparallel = co.parallel(Gstiff,Gsoft)

t, y = co.impulse_response(Gparallel)
plotresponse(t,y)

# %% [markdown]
#
# ---
# # <center> <font color=darkgreen>Time domain simulation</font>  </center>
# ---
#
#

# %% [markdown]
# ## [`initial_response(sys,T=None,X0=0.0)`](https://python-control.readthedocs.io/en/v0.6d/timeresp.html#control.initial_response) Given initial condition, zero input.
# + Returns T, y

# %%
t, y = co.initial_response(Gseries,T=None,X0=1.0)
plotresponse(t,y)

# %% [markdown]
# ## `impulse_response(sys,T=None,...)`: Impulse input, zero initial condition.

# %%
t, y = co.impulse_response(Gseries)
plotresponse(t,y)

# %% [markdown]
# ## [`step_response(sys,T=None,...)`](https://python-control.readthedocs.io/en/v0.6d/timeresp.html#control.step_response): Step input, zero initial condition.

# %%
t, y = co.step_response(Gseries)
plotresponse(t,y)

# %% [markdown]
# ## [ `forced_response(sys,T=None,U=0.0,X0=0.0,...)`](https://python-control.readthedocs.io/en/v0.6d/timeresp.html#control.forced_response): Given input, given initial condition.

# %%
import numpy as np
t = np.linspace(0,10)
u = np.sin(t)
t, y = co.forced_response(sys,t,u)

plt.figure(figsize=(10,4))
plt.plot(t,y,t,u,linewidth=3)
plt.xlabel('t')
plt.grid()
plt.legend(['y','u'])

# %% [markdown]
# ---
# # <center> <font color=darkgreen>System analysis</font>  </center>
# ---
#
#
# # Stability
#
# An equilibrium point of a dynamical system is **asymptotically stable** if small perturbations away from that equilibrium point die out in time. For *linear systems*, an equivalent definition is that all unforced trajectories eventually die out (ie converge to the origin). 
#
# Linear systems are:
# + **asymptotically stable**: if **all** system poles have negative real part. 
# + **unstable**: if **any** system poles all have positive real part. 
#
# **Note**: The remaining cases (ie some have zero real part, none have positive real part) is more complicated.
#
# ## [`pole(sys)`](https://python-control.readthedocs.io/en/v0.6d/analysis.html#control.pole) 

# %%
import math

is_stable = all( co.pole(Gseries).real<0 )

print(is_stable)

# %%
Gundamped = co.tf(1,[1,0,100])

print(co.pole(Gundamped))
 
all( co.pole(Gundamped).real<0 )

# %% [markdown]
#
# ---
# # <center> <font color=darkgreen>Controller design</font>  </center>
# ---

# %% [markdown]
# # Output regulation
#
# ### [`feedback(fwd,fdbck)`](https://python-control.readthedocs.io/en/v0.6d/bdalg_strings.html#control.feedback)
#
#
#
# <img src="../figs/reftrack.svg" width="400">
#
# This block diagram is achieved with,
#
# ```python 
# cl_sys = co.feedback( co.series(cntrl,plant) , 1 )
# ```
#
# ## Proportional feedback

# %%
plant = co.tf(1,[1,1,-2])

cntrl = 4

RtoY = co.feedback( co.series(cntrl,plant) ,1)

plotstepresponse(RtoY)


# %% [markdown]
# ## Proportional-integral feedback

# %%
plant = co.tf(1,[1,1,-2])
cntrl = 4 + 0.5* (1/co.tf('s'))

RtoY = co.feedback( co.series(cntrl,plant) ,1)

plotstepresponse(RtoY)

# %% [markdown]
# ---
# # <center> <font color=darkgreen>Control design example: <br> Balancing the inverted pendulum with pole placement</font>  </center>
# ---
#
# <img src="../figs/cartandpendulum.svg" width="350">

# %% [markdown]
# ## Find the equations of motion ...
# \begin{align}
# \ddot{p} &= \frac{1}{Z}\left( -m\ell\cos\theta\:\xi_1 + \ell\xi_2 \right) \\
# \label{eq:tdd}
# \ddot\theta &= \frac{1}{Z}\left( -(M+m)\xi_1 + \cos\theta\:\xi_2 \right)
# \end{align}
# where
# \begin{align}
# Z &= M\ell + m\ell\sin^2\theta \\
# \label{xi1}
# \xi_1 &= \frac{\sigma}{m\ell}\dot{\theta} - g\sin\theta \\ 
# \label{xi2}
# \xi_2 &= F - c\dot{p} - m\ell\sin\theta\:\dot{\theta}^2 + \frac{\sigma}{\ell}\cos\theta\:\dot{\theta}
# \end{align}
#
# ## Find equilibria ...
#
# These are values of $p$ and $\theta$ such that $\dot{p}=\ddot{p}=\dot\theta=\ddot\theta=F=0$.
#
# Plugging these into the EOM we find that the sole condition on $p$ and $\theta$ for equilibrium is $\sin\theta=0$
#
# The eqilibrium set is all points with $\theta=n\pi$, where $n$ is an integer. 
# \begin{equation} 
# \{ \; (p,\theta) \;|\; p\in\mathbb{R} \; , \; \theta=n\pi, n\in \mathbb{Z} \;\} 
# \end{equation}
#
#
#
# ## Linearize the EOM about the upward-pointing equilibrium state ...
#
# With some work we find the linearized dynamics for the unstable equilibria. 
#
# \begin{equation}
# \frac{d}{dt}
# \begin{bmatrix} \dot{p} \\ p \\ \dot{\theta} \\ \theta \end{bmatrix} = 
# \begin{bmatrix}
# -\frac{c}{M} & 0 & 0 & \frac{m}{M}g \\
# 1 & 0 & 0 & 0 \\
# \mp\frac{c}{M\ell} & 0 & -\frac{\sigma}{m\ell^2} & \pm\frac{M+m}{m\ell}g \\
# 0 & 0 & 1 & 0
# \end{bmatrix}
# \begin{bmatrix} \dot{p} \\ p \\ \dot{\theta} \\ \theta \end{bmatrix} 
# +
# \begin{bmatrix} \frac{1}{M} \\ 0 \\ \pm\frac{1}{M\ell} \\ 0 \end{bmatrix}
#  F
# \end{equation}
#
#
# Introduce some notation to simplify the formaulas.
# \begin{equation}
# \bar{c} = \frac{c}{M}  \quad , \quad
# \bar{\sigma}=\frac{\sigma}{m\ell^2}   \quad , \quad
# \mu = \frac{M}{m} \quad , \quad
# \bar{F} = \frac{F}{M}
# \end{equation}
# Then,
# \begin{equation}
# \frac{d}{dt}
# \begin{bmatrix} \dot{p} \\ p \\ \dot{\theta} \\ \theta \end{bmatrix} = 
# \begin{bmatrix}
# -\bar{c} & 0 & 0 & g/\mu \\
# 1 & 0 & 0 & 0 \\
# \mp\bar{c}/\ell & 0 & -\bar{\sigma} & \pm(1+\mu)g/\ell \\
# 0 & 0 & 1 & 0
# \end{bmatrix}
# \begin{bmatrix} \dot{p} \\ p \\ \dot{\theta} \\ \theta \end{bmatrix}
# +
# \begin{bmatrix} 1 \\ 0 \\ \pm1/\ell \\ 0 \end{bmatrix}
#  \bar{F}
# \end{equation}
#

# %%
M = 0.5;         # mass of the cart
m = 0.2;         # mass of the pendulum
ell = 1;         # length of the pendulum
g = 9.8;         # gravity
c = 1.0;         # translational friction
sigma = 0.01;    # rotational friction

cbar = c/M
sigmabar = sigma/m/ell/ell
mu = M/m

A = [[-cbar,0,0,g/mu],[1,0,0,0],[-cbar/ell,0,-sigmabar,(1+mu)*g/ell],[0,0,1,0]]
B = [[1],[0],[1/ell],[0]]

# %% [markdown]
# ## Check controllability

# %%
np.linalg.matrix_rank(co.ctrb(A,B))==4

# %% [markdown]
# ## Select the closed-loop poles and generate the controller gains

# %%
closed_loop_poles = np.array([-3,-2,-1,-2.5])

# controller
K = co.place(A,B,closed_loop_poles);

print(K)

# %% [markdown]
# ## Create the closed-loop system

# %%
sys_cl = co.feedback( co.ss(A,B,np.eye(4),0) , K )

print(sys_cl)

# %% [markdown]
# ## Test run the system and plot

# %%
t, y = co.initial_response(sys_cl,T=None,X0=[1,1,1,1])

plt.figure(figsize=(10,4))
plt.plot(t,y.T,linewidth=3)
plt.grid()
plt.xlabel('time',fontsize=16)
plt.legend(['$p$','$\dot{p}$',r'$\theta$',r'$\dot\theta$'],fontsize=12);

# %% [markdown]
# ### Comment
#
# This controller will clearly fail on the real nonlinear system (notice that deviations in $\theta$ are reaching over two radians). The real test is whether you can design a controller with this linearized method that will perform well on the nonlinear system as well. 

# %%
