# Optimal Control for a simple pendulum

I'm studying various optimal control methods (and implements them in Matlab), and as test case I choose (for now) a simple pendulum (fixed to the ground), which I want to control to the upper position.

I managed to control it using "simple" feedback method (swing-up based on energy control + LQR stabilization for the upper position), and the state trajectory is show in figure (I forgot the axis description: x is theta, y is theta dot.

Now I want to try a "full" optimal control method, starting with an iterative LQR method (which I found implemented here http://homes.cs.washington.edu/~todorov/software/ilqg_det.m)

The method requires one dynamic function and one cost function (x = [theta; theta_dot], u is the motor torque (one motor only)):

function [xdot, xdot_x, xdot_u] = ilqr_fnDyn(x, u)
xdot = [x(2);
-g/l * sin(x(1)) - d/(m*l^2)* x(2) + 1/(m*l^2) * u];
if nargout > 1
xdot_x = [ 0, 1;
-g/l*cos(x(1)), -d/(m*l^2)];
xdot_u = [0; 1/(m*l^2)];
end
end

function [l, l_x, l_xx, l_u, l_uu, l_ux] = ilqr_fnCost(x, u, t)
%trying J = x_f' Qf x_f + int(dt*[ u^2 ])
Qf = 10000000 * eye(2);
R = 1;
wt = 1;
x_diff = [wrapToPi(x(1) - reference(1)); x(2)-reference(2)];

if isnan(t)
l = x_diff'* Qf * x_diff;
else
l = u'*R*u;
end

if nargout > 1
l_x = zeros(2,1);
l_xx = zeros(2,2);
l_u = 2*R*u;
l_uu = 2 * R;
l_ux = zeros(1,2);

if isnan(t)
l_x = Qf * x_diff;
l_xx = Qf;
end
end
end


Some info on the pendulum: the origin of my system is where the pendulum is fixed to the ground. The angle theta is zero in the stable position (and pi in the unstable/goal position). m is the bob mass, l is the rod length, d is a damping factor (for simplicity I put m=1, l=1, d=0.3)

My cost is simple: penalize the control + the final error.

This is how I call the ilqr function

tspan = [0 10];
dt = 0.01;
steps = floor(tspan(2)/dt);
x0 = [pi/4; 0];
umin = -3; umax = 3;
[x_, u_, L, J_opt ] = ilqg_det(@ilqr_fnDyn, @ilqr_fnCost, dt, steps, x0, 0, umin, umax);


This is the output

Time From 0 to 10. Initial conditions: (0.785398,0.000000). Goal: (-3.141593,0.000000) Length: 1.000000, mass: 1.000000, damping :0.300000

Using Iterative LQR control

Iterations = 5; Cost = 88230673.8003

the nominal trajectory (that is the optimal trajectory the control finds) is

The control is "off"... it doesn't even try to reach the goal... What am I doing wrong? (the algorithm from Todorov seems to work.. at least with his examples)

Without going through all your code (that would be too much like real work), my gut feel is that you have weighted your control effort heavily enough that the least-cost thing to do is to do nothing and live with the error.

Yes, I know -- all your explicit weights are unity. But still -- try giving the control effort a lower weight, or the position error a higher one.

Again without getting deeply into your code, your ilrq function may not "understand" the nonlinear nature of the thing you're controlling. As such, it may not see a way to get to the pendulum upright position, and again, it may fail.

The approach that you first tried, to put just the right amount of energy into the pendulum, then regulate optimally once the pendulum is erect, is probably the best way: you know that in the absence of friction, a system with just the perfectly right amount of energy is going to end up standing still on top (however briefly), so that would seem a sensible place to start.

• Thank you for your comment. As I said commenting the other answer this question is quite old and maybe I should remove it.. The problem is that I never solved it, even because I moved to other algorithms. Regarding your comment about the energy.. The real goal is not to control an inverted pendulum, but to use it as a test bed for ocp algorithms. (low dimension but non-linear and unstable system) – Francesco Jun 27 '13 at 14:26

iLQR is an iterative method but you do not in fact appear to be iterating. Todorov supplies a test script that should elucidate the approach though it may need to be customized for your system.

• The first thing I tried when I implemented the iLQG method is the todorov test and it worked. Now.. this question is of the end of January.. maybe I should close it.. I moved from this method and from matlab to NLP methods – Francesco Jun 27 '13 at 14:23
• I'm sorry I didn't see it sooner. Re: closing it, I would recommend against doing so as others may still find it helpful. – DaemonMaker Jun 27 '13 at 15:38
• @DeamonMaker yeah.. this is the reason why I left it open... :) – Francesco Jun 28 '13 at 8:03