## System dynamics
CartPole environment consists of a cart on a trail and a pole hinge fixed on it. 

### States
$$X = \left[\array{x \\ \dot{x} \\ \theta \\ \dot{\theta} }\right]$$
### Parameters:  
$l = 0.5 m$: half length of pole (homogenuous pole)   
$M = 1 kg$: mass of cart  
$m = 0.1 kg$: mass of pole   
$\tau = 0.02 s$: discrete time peroid   
$F_u (N)$: exterted force on the cart  
### Derivative equations based on Newton and Euler's equations
$$(M+m)\ddot{x}= F_u + ml (\dot{\theta})^2sin\theta-ml\ddot{\theta}cos\theta$$  
$$\frac{4}{3} ml^2\ddot{\theta}=mglsin\theta-mlcos\theta \ddot{x}$$
### How simulation works
- Calculate accelaration based on state $x_k$
$$\gamma_k = \frac{(F_{u_k} + ml{\dot{\theta_k}}^2sin\theta_k)}{M+m}$$   
$$\ddot{\theta_k}=\frac{gsin\theta_k-\gamma_k cos\theta_k}{\frac{4}{3}(M+m)l-mlcos^2\theta_k}$$
$$\ddot{x_k}=\gamma_k-\frac{ml\ddot{\theta_k}cos\theta_k}{M+m}$$
- In very short discrete time interval, calculate $x_{k+1}$
$$x_{k+1} = x_k + \tau\dot{x_k}$$  
$$\dot{x_{k+1}}=\dot{x_k}+\tau\ddot{x_k}$$  
$$\theta_{k+1} = \theta_k + \tau\dot{\theta_k}$$  
$$\dot{\theta_k}=\dot{\theta_k}+\tau\ddot{\theta_k}$$

### Linearized state space function     
- Assumption  
$$sin\theta\approx\theta$$
$$cos\theta\approx 1$$
$$(\dot{\theta})^2 \approx 0$$  
- Take assumption into derivative equations
$$\beta = \frac{4}{3}-\frac{m}{M+m}$$  
$$\ddot{x}_k=-\frac{mg\theta}{(M+m)\beta}\theta-\frac{F_u(\beta(M+m)+m)}{(M+m)^2\beta}$$  
$$\ddot{\theta}_k=\frac{g}{l\beta}\theta-\frac{F_u}{(M+m)l\beta}$$  
- The linearized discrete equation of system following previous procedure
    - states
$$X=\left[\matrix{x \\ \theta \\ \dot{x} \\ \dot{\theta}}\right]$$  
    - state equations
$$x_{k+1}=x_k+\dot{x_k}\tau$$  
$$\theta_{k+1}=\theta_k+\dot{\theta_k}\tau$$  
$$\dot{x}_{k+1} = \dot{x}_k+\tau \ddot{x_k}$$  
$$\dot{\theta}_{k+1} = \dot{\theta}_k+\tau\ddot{\theta}_k$$  
    - discrete state space
$$\left[\matrix{x_{k+1} \\ \dot{x}_{k+1} \\ \theta_{k+1} \\ \dot{\theta}_{k+1}}\right] = 
\left[\matrix{1 & \tau & 0 & 0 \\  
              0 & 1 & -\frac{\tau mg}{(M+m)\beta} & 0 \\  
              0 & 0 & 1 & \tau \\  
              0 & 0 & \frac{g\tau}{l\beta_k}& 1}\right]\left[\matrix{x_{k} \\ \dot{x}_{k} \\ \theta_{k} \\ \dot{\theta}_{k}}\right] + \left[\matrix{0 \\ \frac{(\beta(M+m)+m)}{(M+m)^2\beta} \\ 0 \\ \frac{-1}{(M+m)\beta} }\right][F_{u_k}]$$          


```python
import gym
from cartpole_util import CartPoleEnv
env = CartPoleEnv(1e-1, 0.1) 
```

    [33mWARN: gym.spaces.Box autodetected dtype as <class 'numpy.float32'>. Please provide explicit dtype.[0m



```python
J = 1/12*(env.masspole*2)**2
beta = env.masspole*env.masscart*env.length**2 + J*(env.masspole+env.masscart)
ac = -(env.masspole**2*env.length**2*env.gravity)/beta
bc = (J+env.masspole*env.length**2)/beta
cc = env.masspole*env.length*env.gravity*(env.masscart+env.masspole)/beta
dc = -env.masspole*env.length/beta
```


```python
import numpy as np
A_c = np.array([[0,1,0,0],
              [0,0,ac,0],
              [0,0,0,1],
              [0,0,cc,0]])
B_c = np.array([[0],
                [bc],
                [0],
                [dc]])
C_c = np.array([[0,1,0,0],
               [0,0,0,1]])
D_c = np.array([[0],
               [0]])
```


```python
print(C_c.shape)
```

    (2, 4)



```python
from scipy import signal
sys = signal.StateSpace(A_c, B_c, C_c, D_c)
```


```python
discrete_sys = sys.to_discrete(env.tau)
```


```python
discrete_sys.A
```




    array([[ 1.00000000e+00,  2.00000000e-02, -1.71037389e-04,
            -1.13996348e-06],
           [ 0.00000000e+00,  1.00000000e+00, -1.71144572e-02,
            -1.71037389e-04],
           [ 0.00000000e+00,  0.00000000e+00,  1.00376282e+00,
             2.00250792e-02],
           [-0.00000000e+00, -0.00000000e+00,  3.76518059e-01,
             1.00376282e+00]])




```python
from numpy.linalg import matrix_rank
```


```python
A = discrete_sys.A
# C = discrete_sys.C
# control_C = np.array([])
C= np.array([  [1,1,0,0],
               
               [0,0,0,1]])
```


```python
control_C = np.concatenate((C,np.matmul(C,A)),axis=0)
matrix_rank(control_C)
```




    4




```python
from sympy import sin, cos, Matrix, symbols, diff, simplify
from sympy.abc import rho, phi, theta, tau, omega
```


```python
X = Matrix([rho*cos(phi), rho*sin(phi), rho**2])
```


```python
Y = Matrix([rho, phi])
```


```python
X.jacobian(Y)
```




    Matrix([
    [cos(phi), -rho*sin(phi)],
    [sin(phi),  rho*cos(phi)],
    [   2*rho,             0]])




```python
v, g, F, m, l, M = symbols("v, g, F, m, l, M")
h2 = v+tau*(F+m*l*omega**2*sin(theta)-3/4*m*g*sin(theta)*cos(theta))/(M+m-3/4*m*(cos(theta))**2)
```


```python
simplify(diff(h2, theta))
```




    m*tau*(-1.5*(F - 0.375*g*m*sin(2*theta) + l*m*omega**2*sin(theta))*sin(theta)*cos(theta) + (M - 0.75*m*cos(theta)**2 + m)*(1.5*g*sin(theta)**2 - 0.75*g + 1.0*l*omega**2*cos(theta)))/(M - 0.75*m*cos(theta)**2 + m)**2




```python
diff(h2, omega)
```




    2*l*m*omega*tau*sin(theta)/(M - 0.75*m*cos(theta)**2 + m)




```python
diff(h2, v)
```




    1




```python
h4 = omega+tau*((M+m)*g*sin(theta)-m*l*omega**2*sin(theta)*cos(theta)-F*cos(theta))/(4/3*(m+M)*l-m*l*(cos(theta))**2)
```


```python
diff(h4, omega)
```




    -2*l*m*omega*tau*sin(theta)*cos(theta)/(-l*m*cos(theta)**2 + l*(1.33333333333333*M + 1.33333333333333*m)) + 1




```python

```

70141ed15fe94468e86ceee78accc08e488496e9
ipynb
Jupyter Notebook
Proposal.ipynb
caseypen/MAE_298_Final_Project
66c96aabc7bbcfe070c5170c03d5f6b5196b31bb
MIT
Proposal.ipynb
caseypen/MAE_298_Final_Project
66c96aabc7bbcfe070c5170c03d5f6b5196b31bb
MIT
Proposal.ipynb
caseypen/MAE_298_Final_Project
66c96aabc7bbcfe070c5170c03d5f6b5196b31bb
MIT
2020-11-08T14:38:42.000Z
2020-11-08T14:38:42.000Z
Qwen/Qwen-72B
1. YES
2. YES