# Minimum Energy Double Pendulum Swing Up¶

This is a simple demonstration of trajectory optimization using opty. Given two link pendulum attached to a sliding cart that can be actuated by a lateral force, we look for a minimal energy method to swing the pendulum to a vertical position in a specific time interval.

The following diagram shows an N link pendulum.

Our system is a two link version of this.

# Setup¶

First, import the necessary packages, modules, functions, and classes.

In [1]:
import sympy as sm
import sympy.physics.mechanics as me
import numpy as np
import matplotlib.pyplot as plt
from IPython.core.pylabtools import figsize
from opty.direct_collocation import Problem
from opty.utils import parse_free

import utils

:0: FutureWarning: IPython widgets are experimental and may change in the future.


The following will initialize pretty printing of the symbolics.

In [2]:
me.init_vprinting(use_latex="mathjax")


# Model¶

PyDy includes a canned model for this system. We can create a two link version like so:

In [3]:
pendulum_sys = n_link_pendulum_on_cart(n=2, cart_force=True)

/home/moorepants/anaconda/envs/inverted-pendulum-id/lib/python2.7/site-packages/pydy/system.py:102: PyDyFutureWarning: PyDy System is experimental and may change in the future.
warnings.warn(msg, PyDyFutureWarning)


The system has six states, one exogenous specified input, six and constants.

In [4]:
num_states = len(pendulum_sys.states)

In [5]:
pendulum_sys.states

Out[5]:
$$\left [ q_{0}, \quad q_{1}, \quad q_{2}, \quad u_{0}, \quad u_{1}, \quad u_{2}\right ]$$
In [6]:
pendulum_sys.specifieds_symbols

Out[6]:
$$\left\{F\right\}$$
In [7]:
pendulum_sys.constants_symbols

Out[7]:
$$\left\{g, l_{0}, l_{1}, m_{0}, m_{1}, m_{2}\right\}$$

opty only requires that the equations of motion be provided in implicit form. We will make use of the "full" mass matrix and forcing vector.

In [8]:
M = sm.trigsimp(pendulum_sys.eom_method.mass_matrix_full)
M

Out[8]:
$$\left[\begin{matrix}1 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & m_{0} + m_{1} + m_{2} & - l_{0} \left(m_{1} + m_{2}\right) \operatorname{sin}\left(q_{1}\right) & - l_{1} m_{2} \operatorname{sin}\left(q_{2}\right)\\0 & 0 & 0 & - l_{0} \left(m_{1} + m_{2}\right) \operatorname{sin}\left(q_{1}\right) & l_{0}^{2} m_{1} + l_{0}^{2} m_{2} & l_{0} l_{1} m_{2} \operatorname{cos}\left(q_{1} - q_{2}\right)\\0 & 0 & 0 & - l_{1} m_{2} \operatorname{sin}\left(q_{2}\right) & l_{0} l_{1} m_{2} \operatorname{cos}\left(q_{1} - q_{2}\right) & l_{1}^{2} m_{2}\end{matrix}\right]$$
In [9]:
f = sm.trigsimp(pendulum_sys.eom_method.forcing_full)
f

Out[9]:
$$\left[\begin{matrix}u_{0}\\u_{1}\\u_{2}\\l_{0} m_{1} u^{2}_{1} \operatorname{cos}\left(q_{1}\right) + l_{0} m_{2} u^{2}_{1} \operatorname{cos}\left(q_{1}\right) + l_{1} m_{2} u^{2}_{2} \operatorname{cos}\left(q_{2}\right) + F\\- l_{0} \left(g m_{1} \operatorname{cos}\left(q_{1}\right) + g m_{2} \operatorname{cos}\left(q_{1}\right) + l_{1} m_{2} u^{2}_{2} \operatorname{sin}\left(q_{1} - q_{2}\right)\right)\\l_{1} m_{2} \left(- g \operatorname{cos}\left(q_{2}\right) + l_{0} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{2}\right)\right)\end{matrix}\right]$$

The implicit expression for the right hand side of the equations of motion can be computed as such:

$$\mathbf{0} = \mathbf{g}(\dot{\mathbf{x}}, \mathbf{x}, \mathbf{r}, \mathbf{p}, t) = \mathbf{M} \dot{\mathbf{x}} - \mathbf{f}$$

In [10]:
eom_expr = sm.trigsimp(M * sm.Matrix(pendulum_sys.states).diff() - f)
eom_expr

Out[10]:
$$\left[\begin{matrix}- u_{0} + \dot{q}_{0}\\- u_{1} + \dot{q}_{1}\\- u_{2} + \dot{q}_{2}\\- l_{0} m_{1} u^{2}_{1} \operatorname{cos}\left(q_{1}\right) - l_{0} m_{2} u^{2}_{1} \operatorname{cos}\left(q_{1}\right) - l_{0} \left(m_{1} + m_{2}\right) \operatorname{sin}\left(q_{1}\right) \dot{u}_{1} - l_{1} m_{2} u^{2}_{2} \operatorname{cos}\left(q_{2}\right) - l_{1} m_{2} \operatorname{sin}\left(q_{2}\right) \dot{u}_{2} + \left(m_{0} + m_{1} + m_{2}\right) \dot{u}_{0} - F\\l_{0} l_{1} m_{2} \operatorname{cos}\left(q_{1} - q_{2}\right) \dot{u}_{2} - l_{0} \left(m_{1} + m_{2}\right) \operatorname{sin}\left(q_{1}\right) \dot{u}_{0} + l_{0} \left(g m_{1} \operatorname{cos}\left(q_{1}\right) + g m_{2} \operatorname{cos}\left(q_{1}\right) + l_{1} m_{2} u^{2}_{2} \operatorname{sin}\left(q_{1} - q_{2}\right)\right) + \left(l_{0}^{2} m_{1} + l_{0}^{2} m_{2}\right) \dot{u}_{1}\\l_{1} m_{2} \left(g \operatorname{cos}\left(q_{2}\right) - l_{0} u^{2}_{1} \operatorname{sin}\left(q_{1} - q_{2}\right) + l_{0} \operatorname{cos}\left(q_{1} - q_{2}\right) \dot{u}_{1} + l_{1} \dot{u}_{2} - \operatorname{sin}\left(q_{2}\right) \dot{u}_{0}\right)\end{matrix}\right]$$

# NLP Problem Setup¶

Here we set some numerical values for the constant parameters:

In [11]:
pendulum_sys.constants = {p: 9.81 if p.name == 'g' else 1.0 for p in pendulum_sys.constants_symbols}
pendulum_sys.constants

Out[11]:
$$\left \{ g : 9.81, \quad l_{0} : 1.0, \quad l_{1} : 1.0, \quad m_{0} : 1.0, \quad m_{1} : 1.0, \quad m_{2} : 1.0\right \}$$

We will try to swing the pendulum up in 10 seconds and will discretize time with 500 nodes.

In [12]:
duration = 10.0
num_nodes = 500
interval_value = duration / (num_nodes - 1)


## Objective¶

Our objective is to use minimal energy to swing the pendulum to a vertical stationary state by 10 seconds. One way to approach this is to minimize the square of the applied force:

$$J(\theta) = h \sum F_i^2$$

Here we create a function for the objective and its gradient:

In [13]:
def obj(free):
"""Minimize the sum of the squares of the control force."""
F = free[num_states * num_nodes:]
return interval_value * np.sum(F**2)

grad[num_states * num_nodes:] = 2.0 * interval_value * free[num_states * num_nodes:]


We also need some constraints to specify what state we want the pendulum to be in at the start and end of the task. At the beginning and end we want the pendulum to be stationary, i.e. the velocities should be zero. At the beginning the pendulum should be hanging down, i.e. $q(10)=-\pi/2$ and at the end the pendulum should vertical, i.e. $q(0) = \pi/2$. These constraints are specified by "instance constraints", i.e. constraints specified for a single instance of time.

In [14]:
target_angle = sm.pi / 2

In [15]:
q0, q1, q2, u0, u1, u2 = [x.__class__ for x in pendulum_sys.states]
instance_constraints = (q0(0.0),
q1(0.0) + target_angle,
q1(duration) - target_angle,
q2(0.0) + target_angle,
q2(duration) - target_angle,
u0(0.0),
u0(duration),
u1(0.0),
u1(duration),
u2(0.0),
u2(duration))

instance_constraints

Out[15]:
$$\left ( \operatorname{q0}\left(0.0\right), \quad \operatorname{q1}\left(0.0\right) + \frac{\pi}{2}, \quad \operatorname{q1}\left(10.0\right) - \frac{\pi}{2}, \quad \operatorname{q2}\left(0.0\right) + \frac{\pi}{2}, \quad \operatorname{q2}\left(10.0\right) - \frac{\pi}{2}, \quad \operatorname{u0}\left(0.0\right), \quad \operatorname{u0}\left(10.0\right), \quad \operatorname{u1}\left(0.0\right), \quad \operatorname{u1}\left(10.0\right), \quad \operatorname{u2}\left(0.0\right), \quad \operatorname{u2}\left(10.0\right)\right )$$

## Construct the Problem¶

The Problem object available in opty is now initialized with all of the problem setup information.

In [16]:
prob = Problem(obj, obj_grad, eom_expr, pendulum_sys.states,
num_nodes, interval_value,
known_parameter_map=pendulum_sys.constants,
instance_constraints=instance_constraints)


IPOPT options can be added. The following sets the linear solver.

In [17]:
prob.addOption('linear_solver', 'ma57')


# Solve the Problem¶

We then supply a random initial guess.

In [18]:
initial_guess = np.random.random(prob.num_free)


Alternatively, you can load a previously found optimal solution.

In [19]:
initial_guess = np.load('dp-solution.npy')


The problem can then be solved by calling the solve() method with the initial guess. The solution is returned along with some IPOPT info and stats.

In [20]:
solution, info = prob.solve(initial_guess)

In [21]:
prob.obj(solution)

Out[21]:
$$616.919724446$$

Uncomment this if you want to save an optimal solution.

In [22]:
#np.save('dp-solution', solution)


# Plot the Results¶

In [23]:
x, r, p = parse_free(solution, prob.collocator.num_states,
prob.collocator.num_unknown_input_trajectories, num_nodes)

In [24]:
time = np.linspace(0.0, duration, num=num_nodes)

In [25]:
%matplotlib inline
figsize(12, 9)


## Trajectories¶

In [26]:
fig, axes = plt.subplots(5)

axes[0].set_title('State and Control Trajectories')

axes[0].plot(time, r)
axes[0].set_ylabel('Force [N]')

axes[1].plot(time, x[0])
axes[1].set_ylabel('Distance [m]')
axes[1].legend(pendulum_sys.states[0:1])

axes[2].set_ylabel('Angle [deg]')
axes[2].legend(pendulum_sys.states[1:3])

axes[3].plot(time, x[3])
axes[3].set_ylabel('Speed [m/s]')
axes[3].legend(pendulum_sys.states[3:4])

axes[4].set_ylabel('Angular Rate [deg/s]')
axes[4].legend(pendulum_sys.states[4:])
axes[4].set_xlabel('Time [S]')

Out[26]:
<matplotlib.text.Text at 0x7f1c9b196510>

## Constraint violations¶

In [27]:
con_violations = prob.con(solution)
con_nodes = range(2, num_nodes + 1)
N = len(con_nodes)

fig, axes = plt.subplots(5)

axes[0].set_title('Constraint Violations')
axes[0].plot(con_nodes, con_violations[:N])
axes[0].set_ylabel('Distance [m]')

axes[1].plot(con_nodes, con_violations[N:3 * N].reshape(N, 2))

axes[2].plot(con_nodes, con_violations[3 * N:4 * N])
axes[2].set_ylabel('Speed [m/s]')

axes[3].plot(con_nodes, con_violations[4 * N:6 * N].reshape(N, 2))
axes[3].set_xlabel('Node Number')

axes[4].plot(con_violations[6 * N:])
axes[4].set_ylabel('Instance')

Out[27]:
<matplotlib.text.Text at 0x7f1c9aea7590>

# Motion Visualization¶

In [28]:
scene = utils.pydy_n_link_pendulum_scene(pendulum_sys)

/home/moorepants/anaconda/envs/inverted-pendulum-id/lib/python2.7/site-packages/pydy/viz/camera.py:73: PyDyUserWarning: Rotation of Perspective Camera does not work properly in the visualiser.
warnings.warn(msg, PyDyUserWarning)

In [29]:
scene.states_trajectories = x.T

In [30]:
scene.frames_per_second = 1.0 / interval_value

In [31]:
scene.display_ipython()


# Version Information¶

In [32]:
%install_ext version_information.py
%version_information numpy, sympy, scipy, matplotlib, pydy, opty

Installed version_information.py. To use it, type: