PendAmplCode

param pi := 4*atan(1);

param N := 100;
param time := 5;
param dt := time/N;

param length := 1.0;
param width := 0.1;
param mass := 1.0;
param gravity := 9.81;
param moment_of_inertia_joint := mass*(length*length + width*width)/12
    + mass*length*length/4;

param vmin := -10.0;
param vmax := 10.0;

param umin := -10.0;
param umax := 10.0;

param goal := pi;
param 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;
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;