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
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)