Constrained swing up of the inverted pundulum¶

Reconsider the example of the inverted pendulum in the Usage section.

This example is intended to show how PyTrajectory can handle constraints that affect some state variables. Assume we want to restrict the carts movement along the $x$ -axis to the interval $[-0.8, 0.3]$ that is $\forall t \quad -0.8 \leq x_1(t) \leq 0.3$ (remember: $[x_1, x_2, x_3, x_4] = [x_w, \dot{x_w}, \varphi, \dot{\varphi}]$ ). Furthermore we want the velocity of the cart to be bounded by $[-2.0, 2.0]$ .

To set these constraints PyTrajectory expects a dictionary containing the index of the constrained variables as keys and the box constraints as corresponding values. In our case this dictionary would look like

>>> con = {0 : [-0.8, 0.3], 1 : [-2.0, 2.0]}

(remember that Python starts indexing at $0$ ).

In order to get a solution we raise the translation time from $T = 2[s]$ to $T = 3[s]$ . Next, the only different thing to do is to pass the dictionary when instantiating the trajectory object.

>>> T = Trajectory(f, a, b=3.0, xa, xb, uab, constraints=con)

Source Code¶'''
This example of the inverted pendulum demonstrates how to handle possible state constraints.
'''

# import all we need for solving the problem
from pytrajectory import ControlSystem
import numpy as np
from sympy import cos, sin

# first, we define the function that returns the vectorfield
def f(x,u):
    x1, x2, x3, x4 = x  # system variables
    u1, = u             # input variable
    
    l = 0.5     # length of the pendulum
    g = 9.81    # gravitational acceleration
    
    # this is the vectorfield
    ff = [          x2,
                    u1,
                    x4,
            (1/l)*(g*sin(x3)+u1*cos(x3))]
    
    return ff

# then we specify all boundary conditions
a = 0.0
xa = [0.0, 0.0, np.pi, 0.0]

b = 3.0
xb = [0.0, 0.0, 0.0, 0.0]

ua = [0.0]
ub = [0.0]

# next, this is the dictionary containing the constraints
con = { 0 : [-0.8, 0.3], 
        1 : [-2.0, 2.0] }

# now we create our Trajectory object and alter some method parameters via the keyword arguments
S = ControlSystem(f, a, b, xa, xb, ua, ub, constraints=con, kx=5, use_chains=False)

# time to run the iteration
S.solve()