Optimal Control for a simple pendulum

Question

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)

Answer 1

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.

Answer 2

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.