$\newcommand{\mb}[1]{\mathbf{ #1 }}$
$\newcommand{\bb}[1]{\mathbb{ #1 }}$
$\newcommand{\bs}[1]{\boldsymbol{ #1 }}$
$\newcommand{\norm}[1]{\left\Vert #1 \right\Vert}$

$\newcommand{\der}[2]{\frac{\mathrm{d} #1 }{\mathrm{d} #2 }}$
$\newcommand{\derp}[2]{\frac{\partial #1 }{\partial #2 }}$

$\newcommand{\R}{\bb{R}}$

# Double Inverted Pendulum

Consider a frictionless two-link pendulum. Torque $\tau_1$ is applied at a fixed base, where the first link is attached. Torque $\tau_2$ is applied at the opposite end of the first link, where the second link is attached. 

The first and second links are modeled as massless, with point masses $m_1$ and $m_2$ at lengths of $l_1$ and $l_2$ from the torque applications, respectively.

Assume the angle of the first link from upright $\theta_1$ and the angle of the second link from the first link $\theta_2$ are measured, along with the corresponding angular rates $\dot{\theta}_1$ and $\dot{\theta}_2$. Let $\mb{q} = (\theta_1, \theta_2)$ and $\dot{\mb{q}} = (\dot{\theta}_1, \dot{\theta}_2)$.

## Dynamics

Let $\mathcal{I} = (\mb{i}_1, \mb{i}_2, \mb{i}_3)$ be an inertial reference frame and let $\mathcal{A} = (\mb{a}_1, \mb{a}_2, \mb{a}_3)$ and $\mathcal{B} = (\mb{b}_1, \mb{b}_2, \mb{b}_3)$ be body reference frames such that $\mb{i}_3 = \mb{a}_3 = \mb{b}_3$. Let $\mb{e}_1, \mb{e}_2, \mb{e}_3 \in \R^3$ be elementary basis vectors.

Let $\mb{f}_1^\mathcal{I} = -m_1 g \mb{e}_1$ and $\mb{f}_2^{\mathcal{I}} = -m_2 g \mb{e}_1$ denote graviational forces on the first and second link, respectively, both resolved in the inertial reference frame.

Let $\mb{r}_{10}^\mathcal{A} = l_1 \mb{e}_1$ denote the position of the joint relative to the base resolved in the $\mathcal{A}$ frame. Let $\mb{r}_{21}^\mathcal{B} = l_2 \mb{e}_1$ denote the position of the end of the pendulum relative to the joint resolved in the $\mathcal{B}$ frame.

Let $\mb{R}: \R \to \R^{3 \times 3}$ be a rotation matrix defined as:

\begin{equation}
    \mb{R}(\theta) = \begin{bmatrix} \cos{\theta} & -\sin{\theta} & 0 \\ \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 1 \end{bmatrix},
\end{equation}

for all $\theta \in \R$.

The direction cosine matrix (DCM) relating $\mathcal{A}$ frame coordinates to inertial frame coordinates is $\mb{C}^{\mathcal{A}\mathcal{I}} = \mb{R}(\theta_1)^\top$, while the DCM relating $\mathcal{B}$ frame coordinates to $\mathcal{A}$ frame coordinates is $\mb{C}^{\mathcal{B}\mathcal{A}} = \mb{R}(\theta_2)^\top$.

Since $\mb{R}(\theta)$ is orthogonal for all $\theta \in \R$, $\mb{C}^{\mathcal{I}\mathcal{A}} = \mb{R}(\theta_1)$ and $\mb{C}^{\mathcal{A}\mathcal{B}} = \mb{R}(\theta_2)$.

Additionally, the DCM relating $\mathcal{B}$ frame coordinates to $\mathcal{I}$ frame coordinates is $\mb{C}^{\mathcal{B}\mathcal{I}} = \mb{C}^{\mathcal{B}\mathcal{A}}\mb{C}^{\mathcal{A}\mathcal{I}}$, and $\mb{C}^{\mathcal{I}\mathcal{B}} = \mb{C}^{\mathcal{I}\mathcal{A}}\mb{C}^{\mathcal{A}\mathcal{B}} = \mb{R}(\theta_1)\mb{R}(\theta_2)$.

The position of the joint relative to the base resolved in the inertial frame is:

\begin{align}
    \mb{r}_{10}^\mathcal{I} &= \mb{C}^{\mathcal{I}\mathcal{A}}\mb{r}_{10}^{\mathcal{A}}\\
    &= l_1 \mb{R}(\theta_1)\mb{e}_1\\
    &= \begin{bmatrix} l_1 \cos{\theta} \\ l_1 \sin{\theta} \\ 0 \end{bmatrix},
\end{align}

while the position of the end of the pendulum relative to the base resolved in the inertial frame is:

\begin{align}
    \mb{r}_{20}^\mathcal{I} &= \mb{r}_{21}^\mathcal{I} + \mb{r}_{10}^\mathcal{I}\\
    &= \mb{C}^{\mathcal{IB}}\mb{r}_{21}^\mathcal{B} + \mb{C}^{\mathcal{I}\mathcal{A}}\mb{r}_{10}^\mathcal{A}\\
    &= l_2 \mb{R}(\theta_1)\mb{R}(\theta_2) \mb{e}_1 + l_1 \mb{R}(\theta_1) \mb{e}_1\\
    &= \mb{R}(\theta_1) \begin{bmatrix} l_2 \cos{\theta_2} \\ l_2 \sin{\theta_2} \\ 0 \end{bmatrix} + \begin{bmatrix} l_1 \cos{\theta_1} \\ l_1 \sin{\theta_1} \\ 0 \end{bmatrix}\\
    &= \begin{bmatrix} l_2 \cos{\theta_1} \cos{\theta_2} - l_2 \sin{\theta_1} \sin{\theta_2} + l_1 \cos{\theta_1} \\ l_2 \sin{\theta_1} \cos{\theta_2} + l_2 \cos{\theta_1} \sin{\theta_2} + l_1 \sin{\theta_1} \\ 0 \end{bmatrix}\\
    &= \begin{bmatrix} l_2 \cos{(\theta_1 + \theta_2)} + l_1 \cos{\theta_1} \\ l_2 \sin{(\theta_1 + \theta_2)} + l_1 \sin{\theta_1} \\ 0 \end{bmatrix},
\end{align}

making use of the facts that $\cos{(\theta_1 + \theta_2)} = \cos{\theta_1}\cos{\theta_2} - \sin{\theta_1}\sin{\theta_2}$ and $\sin{(\theta_1 + \theta_2)} = \sin{\theta_1}\cos{\theta_2} + \cos{\theta_1}\sin{\theta_2}$.

The corresponding velocities are:

\begin{equation}
    \dot{\mb{r}}_{10}^\mathcal{I} = \begin{bmatrix} -l_1 \sin{\theta_1} & 0 \\ l_1 \cos{\theta_1} & 0 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \end{bmatrix},
\end{equation}

as well as:

\begin{equation}
    \dot{\mb{r}}_{20}^\mathcal{I} = \begin{bmatrix} -l_2 \sin{(\theta_1 + \theta_2)} -l_1 \sin{\theta_1} & -l_2 \sin{(\theta_1 + \theta_2)} \\ l_2 \cos{(\theta_1 + \theta_2)} + l_1 \cos{\theta_1} & l_2 \cos{(\theta_1 + \theta_2)} \\ 0 & 0 \end{bmatrix} \begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \end{bmatrix},
\end{equation}

The corresponding kinetic energy terms are:

\begin{equation}
    \frac{1}{2}m_1\norm{ \dot{\mb{r}}_{10}^\mathcal{I} }_2^2 = \frac{1}{2} m_1 \dot{\mb{q}}^\top \begin{bmatrix} l_1^2 & 0 \\ 0 & 0 \end{bmatrix} \dot{\mb{q}},
\end{equation}

as well as:

\begin{align}
    \frac{1}{2}m_2\norm{ \dot{\mb{r}}_{20}^\mathcal{I} }_2^2 &= \frac{1}{2} m_2 \dot{\mb{q}}^\top \begin{bmatrix} l_1^2 + 2 l_1 l_2 (\sin{(\theta_1 + \theta_2)}\sin{\theta_1} + \cos{(\theta_1 + \theta_2)}\cos{\theta_1})  + l_2^2 & l_1 l_2 (\sin{(\theta_1 + \theta_2)\sin{\theta_1}} + \cos{(\theta_1 + \theta_2)}\cos{\theta_1}) + l_2^2 \\ l_1 l_2 (\sin{(\theta_1 + \theta_2)\sin{\theta_1}} + \cos{(\theta_1 + \theta_2)}\cos{\theta_1}) + l_2^2 & l_2^2 \end{bmatrix} \dot{\mb{q}}\\
    &= \frac{1}{2}m_2\dot{\mb{q}}^\top \begin{bmatrix} l_1^2 + 2 l_1 l_2 \cos{\theta_2} + l_2^2 & l_1 l_2 \cos{\theta_2} + l_2^2 \\ l_1 l_2 \cos{\theta_2} + l_2^2 & l_2^2 \end{bmatrix} \dot{\mb{q}},
\end{align}

using the fact that $\cos{\theta_2} = \cos{((\theta_1 + \theta_2) - \theta_1)} = \cos{(\theta_1 + \theta_2)}\cos{\theta_1} + \sin{(\theta_1 + \theta_2)}\sin{\theta_1}$.

The total kinetic energy of the system can now be described as:

\begin{equation}
    T(\mb{q}, \dot{\mb{q}}) = \frac{1}{2} \dot{\mb{q}}^\top \underbrace{ \begin{bmatrix} (m_1 + m_2)l_1^2 + 2 m_2 l_1 l_2 \cos{\theta_2} + m_2 l_2^2 & m_2 l_1 l_2 \cos{\theta_2} + m_2 l_2^2 \\ m_2 l_1 l_2 \cos{\theta_2} + m_2 l_2^2 & m_2 l_2^2 \end{bmatrix} }_{\mb{D}(\mb{q})} \dot{\mb{q}}.
\end{equation}

The Coriolis terms matrix can be expressed in terms of the inertia matrix:

\begin{equation}
    \mb{C}(\mb{q}, \dot{\mb{q}}) = \derp{\mb{D}}{\mb{q}}(\cdot, \dot{\mb{q}}, \cdot) - \frac{1}{2}\left( \derp{\mb{D}}{\mb{q}}(\cdot, \dot{\mb{q}}, \cdot) \right)^\top.
\end{equation}

Here, $\derp{\mb{D}}{\mb{q}} \in \R^{2 \times 2 \times 2}$ is represented as a multilinear operator for all $\mb{q} \in \R^2$, with:

\begin{equation}
    \derp{\mb{D}}{\mb{q}}(\mb{e}_i, \mb{e}_j, \mb{e}_k) = \derp{D_{ij}(\mb{q})}{\theta_k},
\end{equation}

for all $i, j, k \in \{1, 2\}$ and elementary basis vectors $\mb{e}_i, \mb{e}_j, \mb{e}_k \in \R^2$. 

We write:

\begin{equation}
    \derp{\mb{D}}{\mb{q}} = \sum_{i=1}^2 \sum_{j=1}^2 \sum_{k=1}^2 \derp{D_{ij}(\mb{q})}{\theta_k} \mb{e}_i \otimes \mb{e}_j \otimes \mb{e}_k,
\end{equation}

and contract along the second argument as:

\begin{align}
    \derp{\mb{D}}{\mb{q}} &= \sum_{i=1}^2 \sum_{j=1}^2 \sum_{k=1}^2 \derp{D_{ij}(\mb{q})}{\theta_k} \dot{\theta}_j \mb{e}_i \otimes \mb{e}_k\\
    &= \sum_{i=1}^2 \sum_{k=1}^2 \left( \sum_{j=1}^2 \derp{D_{ij}(\mb{q})}{\theta_k} \dot{\theta}_j \right) \mb{e}_i \otimes \mb{e}_k\\
    &= \begin{bmatrix} 0 & -2 m_2 l_1 l_2 \dot{\theta}_1 \sin{\theta_2} - m_2 l_1 l_2 \dot{\theta}_2 \sin{\theta_2} \\ 0 & -m_2 l_1 l_2 \dot{\theta}_1 \sin{\theta_2} \end{bmatrix}.
\end{align}

Therefore, the Coriolis terms matrix is:

\begin{equation}
    \mb{C}(\mb{q}, \dot{\mb{q}}) = \begin{bmatrix} 0 & -m_2 l_1 l_2 (2 \dot{\theta}_1 + \dot{\theta}_2) \sin{\theta}_2 \\ \frac{1}{2} m_2 l_1 l_2 (2 \dot{\theta}_1 + \dot{\theta}_2) \sin{\theta}_2 & -\frac{1}{2} m_2 l_1 l_2 \dot{\theta}_1 \sin{\theta}_2 \end{bmatrix}.
\end{equation}

The potential energy is:

\begin{align}
    U(\mb{q}) &= -(\mb{f}_1^\mathcal{I})^\top\mb{r}_{10}^\mathcal{I} - (\mb{f}_2^\mathcal{I})^\top\mb{r}_{20}^\mathcal{I}\\
    &= m_1 g l_1 \cos{\theta_1} + m_2 g (l_2 \cos{(\theta_1 + \theta_2)} + l_1 \cos{\theta_1})\\
    &= (m_1 + m_2) g l_1 \cos{\theta_1} + m_2 g l_2 \cos{(\theta_1 + \theta_2)}.
\end{align}

The gradient of the potential energy is:

\begin{align}
    \mb{G}(\mb{q}) &= \left(\derp{U}{\mb{q}}\right)^\top\\
    &= \begin{bmatrix} -(m_1 + m_2) g l_1 \sin{\theta_1} - m_2 g l_2 \sin{(\theta_1 + \theta_2)} \\ -m_2 g l_2 \sin{(\theta_1 + \theta_2)} \end{bmatrix}.
\end{align}


```python
# %load core/systems/double_inverted_pendulum.py
from numpy import array, cos, identity, sin
from core.dynamics import RoboticDynamics, SystemDynamics

class DoubleInvertedPendulum(SystemDynamics, RoboticDynamics):
    def __init__(self, m_1, m_2, l_1, l_2, g=9.81):
        SystemDynamics.__init__(self, 4, 2)
        RoboticDynamics.__init__(self, identity(2))
        self.params = m_1, m_2, l_1, l_2, g
        
    def D(self, q):
        m_1, m_2, l_1, l_2, _ = self.params
        _, theta_2 = q
        D_11 = (m_1 + m_2) * (l_1 ** 2) + 2 * m_2 * l_1 * l_2 * cos(theta_2) + m_2 * (l_2 ** 2)
        D_12 = m_2 * l_1 * l_2 * cos(theta_2) + m_2 * (l_2 ** 2)
        D_21 = D_12
        D_22 = m_2 * (l_2 ** 2)
        return array([[D_11, D_12], [D_21, D_22]])
    
    def C(self, q, q_dot):
        _, m_2, l_1, l_2, _ = self.params
        _, theta_2 = q
        theta_1_dot, theta_2_dot = q_dot
        C_11 = 0
        C_12 = -m_2 * l_1 * l_2 * (2 * theta_1_dot + theta_2_dot) * sin(theta_2)
        C_21 = -C_12 / 2
        C_22 = -m_2 * l_1 * l_2 * theta_1_dot * sin(theta_2) / 2
        return array([[C_11, C_12], [C_21, C_22]])
    
    def U(self, q):
        m_1, m_2, l_1, l_2, g = self.params
        theta_1, theta_2 = q
        return (m_1 + m_2) * g * l_1 * cos(theta_1) + m_2 * g * l_2 * cos(theta_1 + theta_2)
    
    def G(self, q):
        m_1, m_2, l_1, l_2, g = self.params
        theta_1, theta_2 = q
        G_1 = -(m_1 + m_2) * g * l_1 * sin(theta_1) - m_2 * g * l_2 * sin(theta_1 + theta_2)
        G_2 = -m_2 * g * l_2 * sin(theta_1 + theta_2)
        return array([G_1, G_2])
```


```python
m_1, m_2 = 0.25, 0.25
l_1, l_2 = 0.5, 0.5
dip = DoubleInvertedPendulum(m_1, m_2, l_1, l_2)
```

## Feedback Linearization


```python
from numpy import identity
from core.controllers import FBLinController, LQRController

Q, R = 10 * identity(4), identity(2)
lqr = LQRController.build(dip, Q, R)
fb_lin = FBLinController(dip, lqr)
```


```python
from numpy import linspace

x_0 = array([1, 0, 0, 0])
ts = linspace(0, 10, 1000 + 1)
fb_lin_data = dip.simulate(x_0, fb_lin, ts)
```


```python
from matplotlib.pyplot import figure, grid, plot, show, subplot, xlabel, ylabel
```


```python
xs, us = fb_lin_data

figure(figsize=(12, 8))

subplot(221)
plot(ts, xs[:, 0], linewidth=3)
grid()
xlabel('Time (sec)', fontsize=16)
ylabel('$\\theta_1$', fontsize=16)

subplot(222)
plot(ts, xs[:, 1], linewidth=3)
grid()
xlabel('Time (sec)', fontsize=16)
ylabel('$\\theta_2$', fontsize=16)

subplot(223)
plot(ts[:-1], us[:, 0], linewidth=3)
grid()
xlabel('Time (sec)', fontsize=16)
ylabel('$\\tau_1$', fontsize=16)

subplot(224)
plot(ts[:-1], us[:, 1], linewidth=3)
grid()
xlabel('Time (sec)', fontsize=16)
ylabel('$\\tau_2$', fontsize=16)

show()
```

## Energy-Based Controller


```python
from core.controllers import EnergyController

K_p = identity(2)
K_d = identity(2)
energy = EnergyController(dip, K_p, K_d)
```


```python
energy_data = dip.simulate(x_0, energy, ts)
```


```python
from matplotlib.pyplot import legend

figure(figsize=(12, 8))

subplot(221)
xs, _ = fb_lin_data
plot(ts, xs[:, 0], linewidth=3, label='FBL')
xs, _ = energy_data
plot(ts, xs[:, 0], linewidth=3, label='Energy')
grid()
legend(fontsize=16)
xlabel('Time (sec)', fontsize=16)
ylabel('$\\theta_1$', fontsize=16)

subplot(222)
xs, _ = fb_lin_data
plot(ts, xs[:, 1], linewidth=3, label='FBL')
xs, _ = energy_data
plot(ts, xs[:, 1], linewidth=3, label='Energy')
grid()
legend(fontsize=16)
xlabel('Time (sec)', fontsize=16)
ylabel('$\\theta_2$', fontsize=16)

subplot(223)
_, us = fb_lin_data
plot(ts[:-1], us[:, 0], linewidth=3, label='FBL')
_, us = energy_data
plot(ts[:-1], us[:, 0], linewidth=3, label='Energy')
grid()
legend(fontsize=16)
xlabel('Time (sec)', fontsize=16)
ylabel('$\\tau_1$', fontsize=16)

subplot(224)
_, us = fb_lin_data
plot(ts[:-1], us[:, 1], linewidth=3, label='FBL')
_, us = energy_data
plot(ts[:-1], us[:, 1], linewidth=3, label='Energy')
grid()
legend(fontsize=16)
xlabel('Time (sec)', fontsize=16)
ylabel('$\\tau_2$', fontsize=16)

show()
```

## Optimization-Based Controller


```python
from core.controllers import QPController

qp = QPController.build_care(dip, Q, R)
qp.add_regularizer(fb_lin, 25)
```


```python
qp_data = dip.simulate(x_0, qp, ts)
```


```python
figure(figsize=(12, 8))

subplot(221)
xs, _ = fb_lin_data
plot(ts, xs[:, 0], linewidth=3, label='FBL')
xs, _ = energy_data
plot(ts, xs[:, 0], linewidth=3, label='Energy')
xs, _ = qp_data
plot(ts, xs[:, 0], linewidth=3, label='QP')
grid()
legend(fontsize=16)
xlabel('Time (sec)', fontsize=16)
ylabel('$\\theta_1$', fontsize=16)

subplot(222)
xs, _ = fb_lin_data
plot(ts, xs[:, 1], linewidth=3, label='FBL')
xs, _ = energy_data
plot(ts, xs[:, 1], linewidth=3, label='Energy')
xs, _ = qp_data
plot(ts, xs[:, 1], linewidth=3, label='QP')
grid()
legend(fontsize=16)
xlabel('Time (sec)', fontsize=16)
ylabel('$\\theta_2$', fontsize=16)

subplot(223)
_, us = fb_lin_data
plot(ts[:-1], us[:, 0], linewidth=3, label='FBL')
_, us = energy_data
plot(ts[:-1], us[:, 0], linewidth=3, label='Energy')
_, us = qp_data
plot(ts[:-1], us[:, 0], linewidth=3, label='QP')
grid()
legend(fontsize=16)
xlabel('Time (sec)', fontsize=16)
ylabel('$\\tau_1$', fontsize=16)

subplot(224)
_, us = fb_lin_data
plot(ts[:-1], us[:, 1], linewidth=3, label='FBL')
_, us = energy_data
plot(ts[:-1], us[:, 1], linewidth=3, label='Energy')
_, us = qp_data
plot(ts[:-1], us[:, 1], linewidth=3, label='QP')
grid()
legend(fontsize=16)
xlabel('Time (sec)', fontsize=16)
ylabel('$\\tau_2$', fontsize=16)

show()
```

# Safe Controller

Consider the task of moving the pendulum from configuration $\mb{q}_0 = (1, 0)$ at rest to $\mb{q}_d = (-1, 0)$ while evading an obstacle above the base of the pendulum. Suppose the position of the obstacle relative to the base of the pendulum is resolved in the inertial frame as $\mb{p}^{\mathcal{I}} = R \mb{e}_1$, with $0 < R \leq l_1 + l_2$. Additionally, suppose we must keep the end at least $\delta R$ m away from the obstacle, for $0 < \delta < 1$.

Consider the function:

\begin{equation}
    h(\mb{q}) = \frac{1}{2} \norm{ \mb{r}_{20}^{\mathcal{I}} - \mb{p}^{\mathcal{I}} }_2^2 - \frac{1}{2} \delta^2 R^2.
\end{equation}

This function is positive for any configuration satisfying $\norm{ \mb{r}_{20}^{\mathcal{I}} - \mb{p}^{\mathcal{I}} }_2 \geq \delta R$. Denote the $0$-superlevel set of this function as $\mathcal{C}$.

The corresponding time derivative is:

\begin{equation}
    \dot{h}(\mb{q}, \dot{\mb{q}}) = ( \mb{r}_{20}^{\mathcal{I}} - \mb{p}^{\mathcal{I}} )^\top \dot{\mb{r}}_{20}^\mathcal{I}.
\end{equation}

The second time derivative is:

\begin{equation}
    \ddot{h}(\mb{q}, \dot{\mb{q}}, \mb{u}) = (\dot{\mb{r}}_{20}^{\mathcal{I}})^\top\dot{\mb{r}}_{20}^\mathcal{I} + (\mb{r}_{20}^\mathcal{I} - \mb{p}^\mathcal{I})^\top \ddot{\mb{r}}_{20}^\mathcal{I}
\end{equation}

Note that $\ddot{\mb{r}}_{20}^\mathcal{I}$ can be expressed as:

\begin{equation}
    \ddot{\mb{r}}_{20}^\mathcal{I} = \begin{bmatrix} -l_2 (\dot{\theta}_1 + \dot{\theta}_2) \cos{(\theta_1 + \theta_2)} - l_1 \dot{\theta}_1 \cos{\theta_1} & -l_2 (\dot{\theta}_1 + \dot{\theta}_2) \cos{(\theta_1 + \theta_2)} \\ -l_2 (\dot{\theta}_1 + \dot{\theta}_2) \sin{(\theta_1 + \theta_2)} - l_1 \dot{\theta}_1 \sin{\theta_1} & -l_2 (\dot{\theta}_1 + \dot{\theta}_2) \sin{(\theta_1 + \theta_2)} \\ 0 & 0 \end{bmatrix} \begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \end{bmatrix} + \begin{bmatrix} -l_2 \sin{(\theta_1 + \theta_2)} -l_1 \sin{\theta_1} & -l_2 \sin{(\theta_1 + \theta_2)} \\ l_2 \cos{(\theta_1 + \theta_2)} + l_1 \cos{\theta_1} & l_2 \cos{(\theta_1 + \theta_2)} \\ 0 & 0 \end{bmatrix} \begin{bmatrix} \ddot{\theta}_1 \\ \ddot{\theta}_2 \end{bmatrix},
\end{equation}

that is, $\ddot{\mb{r}}$ is an affine expression in $\ddot{\mb{q}}$. This means $\ddot{h}$ is an affine expression in $\mb{u}$ and can therefore be used in an inequality constraint of an optimization problem.

Pick $\alpha > 0$. Since the pendulum starts from rest, $\dot{\mb{q}}_0 = \mb{0}_2$, and since $\dot{\mb{r}}_{20}^\mathcal{I}$ is linear in $\dot{\mb{q}}$, initially $\dot{h}(\mb{q}_0, \dot{\mb{q}}_0) = 0$. If $h(\mb{q}_0) \geq 0$, that is, the system is initially safe, then:

\begin{equation}
    \dot{h}(\mb{q}_0, \dot{\mb{q}}_0) \geq -\alpha h(\mb{q}_0).
\end{equation}

Picking actions $\mb{u}$ such that:

\begin{align}
    \der{}{t}(\dot{h}(\mb{q}, \dot{\mb{q}}) + \alpha h(\mb{q})) &= \ddot{h}(\mb{q}, \dot{\mb{q}}, \mb{u}) + \alpha \dot{h}(\mb{q}, \dot{\mb{q}})\\
    &\geq -\alpha ( \dot{h}(\mb{q}, \dot{\mb{q}}) + \alpha h(\mb{q}) )\\
    &= -\alpha \dot{h}(\mb{q}, \dot{\mb{q}}) - \alpha^2 h(\mb{q})
\end{align}

for all $\mb{q} \in \mathcal{C}$ and $\dot{\mb{q}} \in \R^2$ ensures the inequality $\dot{h}(\mb{q}, \dot{\mb{q}}) \geq -\alpha h(\mb{q})$ continues to be satisfied by Nagumo's theorem. It it is always possible to pick such actions, then $\dot{h} + \alpha h$ is called a control safety function (CSF).

Applying this reasoning once more, if $\dot{h}(\mb{q}, \dot{\mb{q}}) \geq -\alpha h(\mb{q})$ for all $\mb{q} \in \mathcal{C}$ and $\dot{\mb{q}} \in \R^2$, then $h(\mb{q}) \geq 0$ continues to be satisfied by Nagumo's theorem. This means the actions maintain the safety of system by ensuring $\mb{q} \in \mathcal{C}$, or that $\mathcal{C}$ is a safe set.

Let $\bs{\Lambda} \in \bb{S}_{++}^{2}$ be an action cost, let $\mb{k}: \mathcal{Q} \times \R^2 \to \R^2$ be a baseline controller, let $\rho \geq 0$ be a regularization coefficient. Consider the optimization-based controller solving:

\begin{align}
    \inf_{\substack{\mb{u} \in \R^2 \\ \mb{x} \in \mathcal{Q} \times \R^2}} ~&~ \mb{u}^\top\bs{\Lambda}\mb{u} + \rho \norm{ \mb{u} - \mb{k}(\mb{x}) }_2^2\\
    \mathrm{s.t.} ~&~ \mb{x} = (\mb{q} - \mb{q}_d, \dot{\mb{q}})\\
    &~ \dot{V}(\mb{x}, \mb{u}) \leq -\alpha' V(\mb{x})\\
    &~ \ddot{h}(\mb{q}, \dot{\mb{q}}, \mb{u}) + \alpha \dot{h}(\mb{q}, \dot{\mb{q}}) \geq -\alpha (\dot{h}(\mb{q}, \dot{\mb{q}}) +\alpha h(\mb{q})),
\end{align}

for all $\mb{q} \in \mathcal{Q}$ and $\mb{\dot{\mb{q}}} \in \R^2$. Here, $\mb{x}$ is denoted as a decision variable only for convenience of notation.

The additional constraint may render the optimization problem infeasible. The stability constraint can be relaxed to find safe solutions. Let $\sigma > 0$ be a slack coefficient, and consider the optimization-based controller solving:

\begin{align}
    \inf_{\substack{\mb{u} \in \R^2 \\ \mb{x} \in \mathcal{Q} \times \R^2 \\ \delta \in \R}} ~&~ \mb{u}^\top\bs{\Lambda}\mb{u} + \rho \norm{ \mb{u} - \mb{k}(\mb{x}) }_2^2 + \sigma \delta^2\\
    \mathrm{s.t.} ~&~ \mb{x} = (\mb{q} - \mb{q}_d, \dot{\mb{q}})\\
    &~ \dot{V}(\mb{x}, \mb{u}) \leq -\alpha' V(\mb{x}) + \delta\\
    &~ \ddot{h}(\mb{q}, \dot{\mb{q}}, \mb{u}) + \alpha \dot{h}(\mb{q}, \dot{\mb{q}}) \geq -\alpha (\dot{h}(\mb{q}, \dot{\mb{q}}) +\alpha h(\mb{q})),
\end{align}


```python
from numpy import concatenate, reshape

class SetPointOutput(RoboticDynamics):
    def __init__(self, robotic_dynamics, q_d):
        RoboticDynamics.__init__(self, robotic_dynamics.B)
        self.robotic_dynamics = robotic_dynamics
        self.q_d = q_d
        
    def eval(self, x, t):
        q, q_dot = reshape(x, (2, -1))
        return concatenate([q - self.q_d, q_dot])
        
    def drift(self, x, t):
        return self.robotic_dynamics.drift(x, t)
    
    def act(self, x, t):
        return self.robotic_dynamics.act(x, t)
```


```python
from core.dynamics import AffineQuadCLF
from numpy.linalg import eigvalsh

q_d = array([-1, 0])
set_point = SetPointOutput(dip, q_d)
lyap = AffineQuadCLF.build_care(set_point, Q, R)
slack_coeff = 1e3
alpha = min(eigvalsh(Q)) / max(eigvalsh(lyap.P))
comp = lambda r: alpha * r

lqr = LQRController.build(set_point, Q, R)
fb_lin = FBLinController(set_point, lqr)

qp = QPController(dip, 2)
qp.add_regularizer(fb_lin, 1)
qp.add_stability_constraint(lyap, comp, True, slack_coeff)
```


```python
qp_data = dip.simulate(x_0, qp, ts)
```


```python
xs, us = qp_data

figure(figsize=(12, 8))

subplot(221)
plot(ts, xs[:, 0], linewidth=3)
grid()
xlabel('Time (sec)', fontsize=16)
ylabel('$\\theta_1$', fontsize=16)

subplot(222)
plot(ts, xs[:, 1], linewidth=3)
grid()
xlabel('Time (sec)', fontsize=16)
ylabel('$\\theta_2$', fontsize=16)

subplot(223)
plot(ts[:-1], us[:, 0], linewidth=3)
grid()
xlabel('Time (sec)', fontsize=16)
ylabel('$\\tau_1$', fontsize=16)

subplot(224)
plot(ts[:-1], us[:, 1], linewidth=3)
grid()
xlabel('Time (sec)', fontsize=16)
ylabel('$\\tau_2$', fontsize=16)

show()
```


```python
from numpy import dot
from numpy.linalg import norm
from core.dynamics import AffineDynamics, ScalarDynamics

class SafetyFunction(AffineDynamics, ScalarDynamics):
    def __init__(self, double_inverted_pendulum, height, prop, alpha):
        self.dynamics = double_inverted_pendulum
        self.height = height
        self.prop = prop
        self.alpha = alpha
        self.p = array([self.height, 0, 0])
        
    def r(self, q):
        _, _, l_1, l_2, _ = self.dynamics.params
        theta_1, theta_2 = q
        r_1 = l_2 * cos(theta_1 + theta_2) + l_1 * cos(theta_1)
        r_2 = l_2 * sin(theta_1 + theta_2) + l_1 * sin(theta_1)
        r_3 = 0
        return array([r_1, r_2, r_3])
    
    def r_dot(self, q, q_dot):
        return dot(self.r_ddot_act(q), q_dot)
    
    def r_ddot_drift(self, q, q_dot):
        _, _, l_1, l_2, _ = self.dynamics.params
        theta_1, theta_2 = q
        theta_1_dot, theta_2_dot = q_dot
        drift_1 = -l_2 * ((theta_1_dot + theta_2_dot) ** 2) * cos(theta_1 + theta_2) - l_1 * (theta_1_dot ** 2) * cos(theta_1)
        drift_2 = -l_2 * ((theta_1_dot + theta_2_dot) ** 2) * sin(theta_1 + theta_2) - l_1 * (theta_1_dot ** 2) * sin(theta_1)
        drift_3 = 0
        return array([drift_1, drift_2, drift_3])
    
    def r_ddot_act(self, q):
        _, _, l_1, l_2, _ = self.dynamics.params
        theta_1, theta_2 = q
        act_1 = array([-l_2 * sin(theta_1 + theta_2) - l_1 * sin(theta_1), -l_2 * sin(theta_1 + theta_2)])
        act_2 = array([l_2 * cos(theta_1 + theta_2) + l_1 * cos(theta_1), l_2 * cos(theta_1 + theta_2)])
        act_3 = array([0, 0])
        return array([act_1, act_2, act_3])

    def h(self, q):
        return (norm(self.r(q) - self.p) ** 2 - (self.prop ** 2) * (self.height ** 2)) / 2
    
    def h_dot(self, q, q_dot):
        return dot(self.r(q) - self.p, self.r_dot(q, q_dot))
    
    def h_ddot_drift(self, q, q_dot):
        r_dot = self.r_dot(q, q_dot)
        return dot(r_dot, r_dot) + dot(self.r(q) - self.p, self.r_ddot_drift(q, q_dot))
    
    def h_ddot_act(self, q):
        return dot(self.r(q) - self.p, self.r_ddot_act(q))
        
    def eval(self, x, t):
        q, q_dot = reshape(x, (2, -1))
        return self.h_dot(q, q_dot) + self.alpha * self.h(q)
    
    def drift(self, x, t):
        q, q_dot = reshape(x, (2, -1))
        return self.h_ddot_drift(q, q_dot) + self.alpha * self.h_dot(q, q_dot)
    
    def act(self, x, t):
        q, q_dot = reshape(x, (2, -1))
        return self.h_ddot_act(q)
```


```python
height = l_1 + l_2
prop = 0.2
alpha = 2.5
safety = SafetyFunction(dip, height, prop, alpha)

alpha_stability = min(eigvalsh(Q)) / max(eigvalsh(lyap.P))
comp_stability = lambda r: alpha_stability * r
comp_safety = lambda r: 5 * r

qp = QPController(dip, 2)
qp.add_regularizer(fb_lin, 1)
qp.add_stability_constraint(lyap, comp_stability, True, slack_coeff)
qp.add_safety_constraint(safety, comp_safety)

qp_data = dip.simulate(x_0, qp, ts)
```


```python
from matplotlib.pyplot import axis

xs, us = qp_data
rs = array([safety.r(q) for q in xs[:, :2]])
r_joints = array([array([l_1 * cos(theta_1), l_1 * sin(theta_1), 0]) for theta_1 in xs[:, 0]])
hs = array([safety.h(q) for q in xs[:, :2]])

thetas = linspace(0, 2 * 3.14, 1000)
unsafe = array([array([prop * height * cos(theta), height + prop * height * sin(theta)]) for theta in thetas])
base = array([0, 0])
theta_1_d, theta_2_d = q_d
target = array([l_2 * sin(theta_1_d + theta_2_d) + l_1 * sin(theta_1_d), l_2 * cos(theta_1_d + theta_2_d) + l_1 * cos(theta_1_d)])

figure(figsize=(12, 4))

subplot(121)
plot(rs[:, 1], rs[:, 0], linewidth=3, label='End')
plot(r_joints[:, 1], r_joints[:, 0], linewidth=3, label='Joint')
plot(unsafe[:, 0], unsafe[:, 1], 'r', linewidth=3)
ylabel('$x_1$ (m)', fontsize=16)
xlabel('$x_2$ (m)', fontsize=16)
grid()
legend(fontsize=16)
axis('equal')
axis([-1, 1, -0.2, 1.2])

subplot(122)
plot(ts, hs, linewidth=3)
xlabel('$t$ (sec)', fontsize=16)
ylabel('$h$', fontsize=16)
grid()
```


```python
from IPython.display import HTML
from matplotlib.animation import FuncAnimation

fig = figure()

ax = fig.add_subplot(111)
ax.fill(unsafe[:, 0], unsafe[:, 1], 'r')
ax.plot(target[0], target[1], 'x', markersize=20)
ax.axis('equal')
ax.grid('on')
ax.set_xlim([-1, 1])
ax.set_ylim([-0.2, 1.2])

link1, = ax.plot([], [], linewidth=3)
link2, = ax.plot([], [], linewidth=3)

def init():
    link1.set_data([], [])
    link2.set_data([], [])
    return link1, link2

def update(idx):   
    base = array([0, 0])
    joint = r_joints[idx]
    end = rs[idx]
    link1.set_data([base[1], joint[1]], [base[0], joint[0]])
    link2.set_data([joint[1], end[1]], [joint[0], end[0]])
    return link1, link2

anim = FuncAnimation(fig, update, init_func=init, frames=500, interval=20, blit=True)
```


```python
HTML(anim.to_html5_video())
```








11aa2ee0b4aa12fd867d40d72d2539ff1562baf5
ipynb
Jupyter Notebook
double-inverted-pendulum.ipynb
ivandariojr/core
c4dec054a3e80355ed3812d48ca2bba286584a67
MIT
2021-01-26T21:00:24.000Z
2022-02-28T23:57:50.000Z
double-inverted-pendulum.ipynb
ivandariojr/core
c4dec054a3e80355ed3812d48ca2bba286584a67
MIT
2020-01-28T22:49:18.000Z
2021-12-14T08:34:39.000Z
double-inverted-pendulum.ipynb
ivandariojr/core
c4dec054a3e80355ed3812d48ca2bba286584a67
MIT
2019-06-07T21:31:20.000Z
2021-12-13T01:00:02.000Z
Qwen/Qwen-72B
1. YES
2. YES