# Truck Problem 
# A.E. Bryson: Dynamic Optimization
# Addison-Wesley, 1999
# Problem 10.3.3. p. 397-400
# Discretization by trapezoidal formulation
# Implemented by Dieter Kraft, 
# Munich University of Applied Sciences
# July, 2004


param nh;                   # Number of subintervals
param u_min;                # Bounds on the control
param u_max;
param kappa;
param alpha;

param pi := 4*atan(1);

param xs {1..4};            # Initial values for the state constraints.
param xf {1..4};            # Final values for the state constraints.

var u {0..nh};              # control delta
var x {0..nh,1..4};         # state variables (psi, alpha, x, y)
      
var d >= 0;                 # final distance
var h = d/nh;               # step size

# Cost function

minimize distance: d
 + kappa*sum{i in 1..nh-1} (u[i+1]-u[i])^2/h; 

# Control bounds (algebraic equations)

subject to u_bounds {j in 0..nh}: u_min <= u[j] <= u_max;

# Differential equations

subject to PSI {i in 0..nh-1}:
	x[i+1,1] = x[i,1] + 0.5*h*(u[i+1]+u[i]);

subject to ALPHA {i in 0..nh-1}:
	x[i+1,2] = x[i,2] + 0.5*h*(u[i+1]-sin(x[i+1,2])+u[i]-sin(x[i,2]));

subject to X {i in 0..nh-1}:
	x[i+1,3] = x[i,3] + 0.5*h*(cos(x[i+1,1])+cos(x[i,1]));

subject to Y {i in 0..nh-1}:
	x[i+1,4] = x[i,4] + 0.5*h*(sin(x[i+1,1])+sin(x[i,1]));

# Boundary conditions

subject to bcs {j in 1..4}: x[00,j] = xs[j];
subject to bcf {j in 1..4}: x[nh,j] = xf[j];

data truck.dat;

option solver ipopt; # (Andreas Waechter, CMU/IBM)
# www-124.ibm.com/developerworks/opensource/coin/Ipopt

solve;

display _total_solve_time;
printf  {k in 0..nh}: "%10.5f %10.5f %10.5f %10.5f %10.5f  %10.5f \n",
 k/nh*d, x[k,1], x[k,2], x[k,3], x[k,4], u[k] > truck.out;