Difference between revisions of "Robot arm problem (TACO)"

# ----------------------------------------------------------------
# Robot arm problem using AMPL and TACO
# (c) Christian Kirches, Sven Leyffer
#
# Source: COPS 3.1 collocation formulation - March 2004
#         David Bortz - Summer 1998          
# ----------------------------------------------------------------
include OptimalControl.mod;
 
# Final positions of the length and the angles for the robot arm
 
param L;		# total length of arm
param pi := 4*atan(1);
 
# Upper bounds on the controls
 
param max_u_rho;
param max_u_the;
param max_u_phi;
 
# Initial positions of the length and the angles for the robot arm
 
param rho_0;
param the_0;
param phi_0;
 
# Final positions of the length and the angles for the robot arm
 
param rho_n;
param the_n;
param phi_n;
 
# The length and the angles theta and phi for the robot arm.
 
var rho >=0, <= L; 
var the >= -pi, <= pi; 
var phi >=0, <= pi;
 
# The derivatives of the length and the angles.
 
var rho_dot;
var the_dot;
var phi_dot;
 
# The controls.
 
var u_rho >= -max_u_rho, <= max_u_rho, suffix type "u0";
var u_the >= -max_u_the, <= max_u_the, suffix type "u0";
var u_phi >= -max_u_phi, <= max_u_phi, suffix type "u0";
 
# The independent time and the final time.
 
var t;
var tf := 1.0, >= 0.1, <= 12 suffix scale 10.0;
 
# The moments of inertia.
 
var I_the = ((L-rho)^3+rho^3)*(sin(phi))^2/3.0;
var I_phi = ((L-rho)^3+rho^3)/3.0;
 
# The robot arm problem.
 
minimize time: eval(t,tf) suffix scale 1e+2;
 
subject to rho_eqn:
        diff(rho, t) = rho_dot;
 
subject to the_eqn:
        diff(the, t) = the_dot;
 
subject to phi_eqn:
        diff (phi, t) = phi_dot;
 
subject to u_rho_eqn:
        diff (rho_dot, t) = u_rho/L;
 
subject to u_the_eqn:
        diff (the_dot, t) = u_the/I_the;
 
subject to u_phi_eqn:
        diff (phi_dot, t) = u_phi/I_phi;
 
# Boundary Conditions
 
subject to rho_0_eqn: eval (rho, 0) = 4.5 suffix type "dpc";
subject to the_0_eqn: eval (the, 0) = 0 suffix type "dpc";
subject to phi_0_eqn: eval (phi, 0) = pi/4 suffix type "dpc";
 
subject to rho_f_eqn: eval (rho, tf) = 4.5;
subject to the_f_eqn: eval (the, tf) = 2*pi/3;
subject to phi_f_eqn: eval (phi, tf) = pi/4;
 
subject to rho_dot_0_eqn: eval (rho_dot, 0) = 0 suffix type "dpc";
subject to the_dot_0_eqn: eval (the_dot, 0) = 0 suffix type "dpc";
subject to phi_dot_0_eqn: eval (phi_dot, 0) = 0 suffix type "dpc";
 
subject to rho_dot_f_eqn: eval (rho_dot, tf) = 0;
subject to the_dot_f_eqn: eval (the_dot, tf) = 0;
subject to phi_dot_f_eqn: eval (phi_dot, tf) = 0;
 
option solver ...;
 
solve;