# Objective: linear
# Constraints: nonconvex nonlinear

#######################################################################
# A robot arm as depicted in Monika's thesis at IWR.
# Letting the pivot point of the robot be the origin of the coordinate
# system and using spherical coordinates, one of the rotational modes
# corresponds to changes in lat and the other corresponds to changes
# in lon (these are the two angles of spherical coordinates).  Also, the
# length, rho, of the arm can change.  We assume that the arm is a rigid
# bar of length 2L that protrudes a distance L+rho from the origin to the
# gripping end and sticks out a distance L-rho in the other direction
# (see the pictures at Monika's web site).  
#
# This model incorporates coriolis and centrifugal forces.
#######################################################################

#######################################################################
# Here is the model:

param pi := 3.14159;
param g := 9.81;	# acceleration due to gravity

param n;		# number of intervals in which time is discretized into

param L;		# half-length of arm
param m_L;		# mass of load
param m_B;		# mass of bar (unloaded arm)
param m_LB := m_L + m_B;		
param I_1_23;		# moment of inertia of bar about bar axis
param I_2_23;		# 
param I_3_23;		#
param I_3_1;		# moment of inertia of base about base axis

param max_F;
param max_T_lon;
param max_T_lat;

param rho_0;		# initial position
param lon_0;		# initial position (theta)
param lat_0;		# initial position

param rho_n;		# final   position
param lon_n;		# final   position
param lat_n;		# final   position

set Np := {0..n};		# discrete times for position
set Nv := {0.5..n-0.5 by 1};	# discrete times for velocity
set Na := {1..n-1};		# discrete times for acceleration

param F {Na};
param T_lon {Na};
param T_lat {Na};

var rho {Np} >= -L, <= L; 	# position
var lon {Np} >= -pi, <= pi; 	# position
var lat {Np} >= -pi/2, <= pi/2; # position

var T >=0;		# total time

# velocities
var rho_dot {i in Nv} = n*(rho[i+0.5] - rho[i-0.5])/T;
var lon_dot {i in Nv} = n*(lon[i+0.5] - lon[i-0.5])/T;
var lat_dot {i in Nv} = n*(lat[i+0.5] - lat[i-0.5])/T;

# accelerations 
var rho_dot2 {i in Na} = n*(rho_dot[i+0.5] - rho_dot[i-0.5])/T;
var lon_dot2 {i in Na} = n*(lon_dot[i+0.5] - lon_dot[i-0.5])/T;
var lat_dot2 {i in Na} = n*(lat_dot[i+0.5] - lat_dot[i-0.5])/T;

# lengths and moments of inertia 
var s   {i in Na} = rho[i] + m_L*L/m_LB;
var a   {i in Na} # = I_3_23 - I_2_23 + m_B*rho[i]^2 + m_L*(rho[i] + L)^2;
                  = ((L-rho[i])^3 + (L+rho[i])^3) * m_B / (3*2*L);
var a_1 {i in Na} # = I_3_1 + I_2_23 + a[i]*cos(lat[i])^2;
	  = ((L-rho[i])^3 + (L+rho[i])^3) * cos(lat[i])^2 * m_B / (3*2*L);
var a_2 {i in Na} # = I_1_23 + m_B*rho[i]^2 + m_L*(rho[i] + L)^2;
                  = ((L-rho[i])^3 + (L+rho[i])^3) * m_B / (3*2*L);

minimize time: T;

subject to F_bnds {i in Na}: 
	-max_F <= 

		m_LB * 
		(
		rho_dot2[i] 
		- s[i]*
		    ( ((lon_dot[i-0.5]+lon_dot[i+0.5])^2)/4 * cos(lat[i])^2 
		    + ((lat_dot[i-0.5]+lat_dot[i+0.5])^2)/4 )
		+ g * sin(lat[i]) 
		)
	       <= max_F;

subject to T_lon_bnds {i in Na}: 
	-max_T_lon <=
		a_1[i] * lon_dot2[i] 
		- 2 * a[i] * sin(lat[i]) * cos(lat[i]) 
		    * (lon_dot[i-0.5]+lon_dot[i+0.5]) 
		    * (lat_dot[i-0.5]+lat_dot[i+0.5])/4
		+ 2 * m_LB * s[i] * cos(lat[i])^2 
		    * (rho_dot[i-0.5]+rho_dot[i+0.5])
		    * (lon_dot[i-0.5]+lon_dot[i+0.5])/4
		<= max_T_lon;

subject to T_lat_bnds {i in Na}: 
	-max_T_lat <=
		a_2[i] * lat_dot2[i] 
		+ a[i] * sin(lat[i]) * cos(lat[i]) 
		       * ((lon_dot[i-0.5]+lon_dot[i+0.5])^2)/4
		+ 2 * m_LB * s[i] 
		    * (rho_dot[i-0.5]+rho_dot[i+0.5])
		    * (lat_dot[i-0.5]+lat_dot[i+0.5])/4
		+ m_LB * g * s[i] * cos(lat[i])
		<= max_T_lat;

subject to init_rho: rho[0] = rho_0;
subject to init_lon: lon[0] = lon_0;
subject to init_lat: lat[0] = lat_0;

subject to finl_rho: rho[n] = rho_n;
subject to finl_lon: lon[n] = lon_n;
subject to finl_lat: lat[n] = lat_n;

subject to init_rho_dot: rho_dot[0.5] = 0;
subject to init_lon_dot: lon_dot[0.5] = 0;
subject to init_lat_dot: lat_dot[0.5] = 0;

subject to finl_rho_dot: rho_dot[n-0.5] = 0;
subject to finl_lon_dot: lon_dot[n-0.5] = 0;
subject to finl_lat_dot: lat_dot[n-0.5] = 0;

#######################################################################
# Here is data corresponding to a specific rotation.

data;

let n := 50;

let L      := 1.18; #:=  0.75;		# half-length of arm
let m_L    :=  0;		# mass of load
let m_B    := 40;		# mass of bar (unloaded arm)
let I_1_23 := 18.5;		# moment of inertia of bar about hor axis
let I_2_23 :=  0.0; #:=  0.12;		# 
let I_3_23 := 18.5;		#
let I_3_1  :=  0.0;		# moment of inertia of base about base axis

let max_F     := 5;
let max_T_lon := 300;
let max_T_lat := 300;

let rho_0 := 0.4*L;
let lon_0 := 0;
let lat_0 := pi/4;

let rho_n := 0.4*L;
let lon_n := 2*pi/3;
let lat_n := pi/4;

#######################################################################
# In order to get convergence, it seems to be necessary to initialize
# to something sort of reasonable:

let {i in Np} rho[i] := 
    if (i in 1..n/2) then rho_0 + 2*(rho_n-rho_0)*(i-1)^2/(n-2)^2
		     else rho_n + 2*(rho_0-rho_n)*(i-n+1)^2/(n-2)^2;
let {i in Np} lon[i] := 
    if (i in 1..n/2) then lon_0 + 2*(lon_n-lon_0)*(i-1)^2/(n-2)^2
		     else lon_n + 2*(lon_0-lon_n)*(i-n+1)^2/(n-2)^2;
let {i in Np} lat[i] := 
    if (i in 1..n/2) then lat_0 + 2*(lat_n-lat_0)*(i-1)^2/(n-2)^2
		     else lat_n + 2*(lat_0-lat_n)*(i-n+1)^2/(n-2)^2;

let rho[0] := rho_0;
let lon[0] := lon_0;
let lat[0] := lat_0;
let rho[n] := rho_n;
let lon[n] := lon_n;
let lat[n] := lat_n;

#######################################################################
# And now we solve it using LOQO.  Note that some parameter settings
# had to be changed from their defaults.

#option solver loqo;
#option loqo_options "pred_corr=0 mufactor=0.0 steplen=0.9 sigfig=6 \
#	verbose=2 iterlim=200 bndpush=1";
#option solver minos;

param T_max;
param T_min;
let T_max := 5.00;
let T_min := 0;

#################################################################
# declare variables to store best primal/dual solution found so far

param save_rho {Np};
param save_lon {Np};
param save_lat {Np};

param     save_F_bnds {Na};
param save_T_lon_bnds {Na};
param save_T_lat_bnds {Na};

param save_init_rho;
param save_init_lon;
param save_init_lat;

param save_finl_rho;
param save_finl_lon;
param save_finl_lat;

param save_init_rho_dot;
param save_init_lon_dot;
param save_init_lat_dot;

param save_finl_rho_dot;
param save_finl_lon_dot;
param save_finl_lat_dot;

#################################################################
# initialize variables to store best primal/dual solution found so far

let {i in Np} save_rho[i] := 
    if (i in 1..n/2) then rho_0 + 2*(rho_n-rho_0)*(i-1)^2/(n-2)^2
		     else rho_n + 2*(rho_0-rho_n)*(i-n+1)^2/(n-2)^2;
let {i in Np} save_lon[i] := 
    if (i in 1..n/2) then lon_0 + 2*(lon_n-lon_0)*(i-1)^2/(n-2)^2
		     else lon_n + 2*(lon_0-lon_n)*(i-n+1)^2/(n-2)^2;
let {i in Np} save_lat[i] := 
    if (i in 1..n/2) then lat_0 + 2*(lat_n-lat_0)*(i-1)^2/(n-2)^2
		     else lat_n + 2*(lat_0-lat_n)*(i-n+1)^2/(n-2)^2;

let save_rho[0] := rho_0;
let save_lon[0] := lon_0;
let save_lat[0] := lat_0;
let save_rho[n] := rho_n;
let save_lon[n] := lon_n;
let save_lat[n] := lat_n;

let {i in Na}     save_F_bnds[i] := 0;
let {i in Na} save_T_lon_bnds[i] := 0;
let {i in Na} save_T_lat_bnds[i] := 0;

let save_init_rho := 0;
let save_init_lon := 0;
let save_init_lat := 0;

let save_finl_rho := 0;
let save_finl_lon := 0;
let save_finl_lat := 0;

let save_init_rho_dot := 0;
let save_init_lon_dot := 0;
let save_init_lat_dot := 0;

let save_finl_rho_dot := 0;
let save_finl_lon_dot := 0;
let save_finl_lat_dot := 0;

#################################################################
# begin binary search

#repeat while T_max - T_min > 0.01 {
#    fix T := (T_max + T_min)/2;
#    display T;
#    solve;
#    if match(solve_message, "optimal") == 0 then {
#	let {i in Np} rho[i] := save_rho[i];
#	let {i in Np} lon[i] := save_lon[i];
#	let {i in Np} lat[i] := save_lat[i];
#
#	let {i in Na}     F_bnds[i] :=     save_F_bnds[i];
#	let {i in Na} T_lon_bnds[i] := save_T_lon_bnds[i];
#	let {i in Na} T_lat_bnds[i] := save_T_lat_bnds[i];
#
#	let init_rho := save_init_rho;
#	let init_lon := save_init_lon;
#	let init_lat := save_init_lat;
#
#	let finl_rho := save_finl_rho;
#	let finl_lon := save_finl_lon;
#	let finl_lat := save_finl_lat;
#
#	let init_rho_dot := save_init_rho_dot;
#	let init_lon_dot := save_init_lon_dot;
#	let init_lat_dot := save_init_lat_dot;
#
#	let finl_rho_dot := save_finl_rho_dot;
#	let finl_lon_dot := save_finl_lon_dot;
#	let finl_lat_dot := save_finl_lat_dot;
#
#	let T_min := T;
#    } else {
#        let {i in Na}     F[i] :=     F_bnds[i].body;
#        let {i in Na} T_lon[i] := T_lon_bnds[i].body;
#        let {i in Na} T_lat[i] := T_lat_bnds[i].body;
#
#	let {i in Np} save_rho[i] := rho[i];
#	let {i in Np} save_lon[i] := lon[i];
#	let {i in Np} save_lat[i] := lat[i];
#
#	let {i in Na}     save_F_bnds[i] :=     F_bnds[i];
#	let {i in Na} save_T_lon_bnds[i] := T_lon_bnds[i];
#	let {i in Na} save_T_lat_bnds[i] := T_lat_bnds[i];
#
#	let save_init_rho := init_rho;
#	let save_init_lon := init_lon;
#	let save_init_lat := init_lat;
#
#	let save_finl_rho := finl_rho;
#	let save_finl_lon := finl_lon;
#	let save_finl_lat := finl_lat;
#
#	let save_init_rho_dot := init_rho_dot;
#	let save_init_lon_dot := init_lon_dot;
#	let save_init_lat_dot := init_lat_dot;
#
#	let save_finl_rho_dot := finl_rho_dot;
#	let save_finl_lon_dot := finl_lon_dot;
#	let save_finl_lat_dot := finl_lat_dot;
#
#	let T_max := T;
#    }
#}

fix T := 2.26;
display T;

param dT;
let dT := T/2;

solve;

option loqo_options "pred_corr=0 mufactor=0.0 steplen=0.9 sigfig=8 \
    verbose=2 inftol=1 iterlim=200 bndpush=0.1";
repeat while dT > 0.01 {

    repeat while match(solve_message, "optimal") > 0 {
        let {i in Na}     F[i] :=     F_bnds[i].body;
        let {i in Na} T_lon[i] := T_lon_bnds[i].body;
        let {i in Na} T_lat[i] := T_lat_bnds[i].body;
    
        let {i in Np} save_rho[i] := rho[i];
        let {i in Np} save_lon[i] := lon[i];
        let {i in Np} save_lat[i] := lat[i];
    
        let {i in Na}     save_F_bnds[i] :=     F_bnds[i];
        let {i in Na} save_T_lon_bnds[i] := T_lon_bnds[i];
        let {i in Na} save_T_lat_bnds[i] := T_lat_bnds[i];
    
        let save_init_rho := init_rho;
        let save_init_lon := init_lon;
        let save_init_lat := init_lat;
    
        let save_finl_rho := finl_rho;
        let save_finl_lon := finl_lon;
        let save_finl_lat := finl_lat;
    
        let save_init_rho_dot := init_rho_dot;
        let save_init_lon_dot := init_lon_dot;
        let save_init_lat_dot := init_lat_dot;
    
        let save_finl_rho_dot := finl_rho_dot;
        let save_finl_lon_dot := finl_lon_dot;
        let save_finl_lat_dot := finl_lat_dot;
    
        fix T := T - dT;
    
        solve;
    }

    let {i in Np} rho[i] := save_rho[i];
    let {i in Np} lon[i] := save_lon[i];
    let {i in Np} lat[i] := save_lat[i];

    let {i in Na}     F_bnds[i] :=     save_F_bnds[i];
    let {i in Na} T_lon_bnds[i] := save_T_lon_bnds[i];
    let {i in Na} T_lat_bnds[i] := save_T_lat_bnds[i];

    let init_rho := save_init_rho;
    let init_lon := save_init_lon;
    let init_lat := save_init_lat;

    let finl_rho := save_finl_rho;
    let finl_lon := save_finl_lon;
    let finl_lat := save_finl_lat;

    let init_rho_dot := save_init_rho_dot;
    let init_lon_dot := save_init_lon_dot;
    let init_lat_dot := save_init_lat_dot;

    let finl_rho_dot := save_finl_rho_dot;
    let finl_lon_dot := save_finl_lon_dot;
    let finl_lat_dot := save_finl_lat_dot;

    let dT := dT/2;
    fix T := T + dT;
    let dT := dT/2;
    solve;
}

#fix T := 2.26;
#display T;
#solve;
#repeat while match(solve_message, "optimal") > 0 {
#    let {i in Na}     F[i] :=     F_bnds[i].body;
#    let {i in Na} T_lon[i] := T_lon_bnds[i].body;
#    let {i in Na} T_lat[i] := T_lat_bnds[i].body;
#
#    display F_bnds[1].lb, F_bnds[1].body, F_bnds[1].ub;
#
#    fix T := T - 0.1;
#
#    option loqo_options "pred_corr=0 mufactor=0.0 steplen=0.9 sigfig=8 \
#	verbose=2 inftol=1 iterlim=200 bndpush=0.1";
#    solve;
#}

#option loqo_options "pred_corr=0 mufactor=0.0 steplen=0.9 sigfig=6 \
#	iterlim=60 verbose=2 bndpush=0";
#unfix T;
#let T := (T_max + T_min)/2;
#solve;

option display_eps 0.0001;

printf {j in Np}: "%4d, %10.5f %10.5f %10.5f \n", j, rho[j], the[j], phi[j];
display rho_dot, lon_dot, lat_dot;
display rho_dot2, lon_dot2, lat_dot2;

display F, T_lon, T_lat;

display sum {i in Na} F[i], sum {i in Na} T_lon[i], sum {i in Na} T_lat[i];