Trajectory Optimization for One-link Pendulum Swing-up

%%% pend1.mod in Matlab
%%% 2008/09/25
%%param
function [res,fval,exitflag]=opt(N)
pi = 4*atan(1);
%N = 100;
time = 5;
dt = time/N;

length = 1.0;
width = 0.1;
mass = 1.0;
gravity = 9.81;
moment_of_inertia_joint = mass*(length*length + width*width)/12 + mass*length*length/4;
vmin = -10.0;
vmax = 10.0;

umin = -10.0;
umax = 10.0;

goal = pi;
state_penalty = 0.1;
%%
% var x{0..N} >= -pi, <= goal + pi;
% var v_offset{i in 0..N-1} = (x[i+1]-x[i])/dt;
% var v{i in 1..N-1} = (v_offset[i]+v_offset[i-1])/2;
% var a{i in 1..N-1} = (v_offset[i]-v_offset[i-1])/dt;
% var u {i in 1..N-1} >=umin, <=umax, :=0.0;

% minimize energy:
%          sum {i in 1..N-1} (state_penalty*(x[i]-goal)*(x[i]-goal) + u[i]*u[i])*dt;

i=0:N;
x0=goal*i/N;
x0=[x0 zeros(1,N-1)];

[res,fval,exitflag]=fmincon(@J,x0,[],[],[],[],[],[],@mycon)

%%
function resJ=J(x)
    resJ=0;
    u=x(N+2:2*N); %N-1 
    for i=1:N-1 % note the last index is N-1
        resJ=resJ+(state_penalty*(x(i+1)-goal)*(x(i+1)-goal) + u(i)*u(i))*dt;
    end
end
%%
function [c,ceq]=mycon(x)
u=x(N+2:2*N);
%s.t. newton
idx=0;
for i=1:N-1
    idx=idx+1;
    a(i)=(x(i+1+1)-2*x(i+1)+x(i-1+1))/(dt^2);
    tce(idx)=u(i) - mass*gravity*length*0.5*sin(x(i+1))-moment_of_inertia_joint*a(i);
end
%s.t. x_init
    idx=idx+1;
    tce(idx)=x(1);
%s.t. x_final
    idx=idx+1;
    tce(idx)=x(N+1)-goal;
%s.t. v_init
    idx=idx+1;
    tce(idx)=x(2)-x(1);
%s.t. v_final:
    idx=idx+1;
    tce(idx)=x(N+1)-x(N);
%s.t. v_max    
    idx=0;
    for i=1:N-1
       idx=idx+1;
       tc(idx)=(x(i+1+1)-x(i-1+1))/(2*dt)-vmax;
    end
%s.t v_min    
    for i=1:N-1
       idx=idx+1;
       tc(idx)=-(x(i+1+1)-x(i-1+1))/(2*dt)+vmin;
    end
    ceq=tce';
    c=tc';
end
%%
%          
% s.t. newton {i in 1..N-1}:
%      dt*(u[i] - mass*gravity*length*0.5*sin(x[i])) = dt*(moment_of_inertia_joint*a[i]);
% s.t. x_init: x[0] = 0;
% s.t. x_final: x[N] = goal;
% s.t. v_init: v_offset[0] = 0;
% s.t. v_final: v_offset[N-1] = 0;
% s.t. v_max {i in 1..N-1}: v[i] <= vmax;
% s.t. v_min {i in 1..N-1}: v[i] >= vmin;
% data;
% #initial guess
% let {i in 0..N} x[i] := goal*i/N;
end