Swing up of the inverted dual pendulum¶

In this example we add another pendulum to the cart in the system.

The system has the state vector $x = [x_1, \dot{x}_1, \varphi_1, \dot{\varphi}_1, \varphi_2, \dot{\varphi}_2]$ . A partial linearization with $y = x_1$ yields the following system state representation where $\tilde{u} = \ddot{y}$ .

$\begin{eqnarray*} \dot{x}_1 & = & x_2 \\ \dot{x}_2 & = & \tilde{u} \\ \dot{x}_3 & = & x_4 \\ \dot{x}_4 & = & \frac{1}{l_1}(g \sin(x_3) + \tilde{u} \cos(x_3)) \\ \dot{x}_5 & = & x_6 \\ \dot{x}_6 & = & \frac{1}{l_2}(g \sin(x_5) + \tilde{u} \cos(x_5)) \end{eqnarray*}$

Here a trajectory should be planned that transfers the system between the following two positions of rest. At the beginning both pendulums should be directed downwards ( $\varphi_1 = \varphi_2 = \pi$ ). After a operating time of $T = 2 [s]$ the cart should be at the same position again and the pendulums should be at rest with $\varphi_1 = \varphi_2 = 0$ .

$\begin{equation*} x(0) = \begin{bmatrix} 0 \\ 0 \\ \pi \\ 0 \\ \pi \\ 0 \end{bmatrix} \rightarrow x(T) = \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} \end{equation*}$

Source Code¶# swing up of the inverted dual pendulum with partial linearization

# import trajectory class and necessary dependencies
from pytrajectory import ControlSystem
from sympy import cos, sin
import numpy as np

# define the function that returns the vectorfield
def f(x,u):
    x1, x2, x3, x4, x5, x6 = x  # system variables
    u, = u                      # input variable
    
    # length of the pendulums
    l1 = 0.7
    l2 = 0.5
    
    g = 9.81    # gravitational acceleration
    
    ff = np.array([         x2,
                            u,
                            x4,
                (1/l1)*(g*sin(x3)+u*cos(x3)),
                            x6,
                (1/l2)*(g*sin(x5)+u*cos(x5))
                    ])
    
    return ff

# system state boundary values for a = 0.0 [s] and b = 2.0 [s]
xa = [0.0, 0.0,  np.pi, 0.0,  np.pi, 0.0]
xb = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

# boundary values for the input
ua = [0.0]
ub = [0.0]

# create trajectory object
S = ControlSystem(f, a=0.0, b=2.0, xa=xa, xb=xb, ua=ua, ub=ub)

# alter some method parameters to increase performance
S.set_param('su', 10)
S.set_param('eps', 8e-2)

# run iteration
S.solve()