Robot Arm

robot.dat

param max_u_rho := 1;
param max_u_the := 1;
param max_u_phi := 1;

param L := 5;

let tf := 1.0;

let {k in 0..nh} rho[k] := 4.5;
let {k in 0..nh} the[k] := (2*pi/3)*(k/nh)^2;
let {k in 0..nh} phi[k] := pi/4;

let {k in 0..nh} rho_dot[k] := 0.0;
let {k in 0..nh} the_dot[k] := (4*pi/3)*(k/nh);
let {k in 0..nh} phi_dot[k] := 0.0;

let {k in 0..nh} u_rho[k] := 0.0;
let {k in 0..nh} u_the[k] := 0.0;
let {k in 0..nh} u_phi[k] := 0.0;

robot.mod

# Robot Arm Problem
# Trapezoidal formulation
# David Bortz - Summer 1998
# COPS 2.0 - September 2000
# COPS 3.0 - November 2002
# COPS 3.1 - March 2004

param nh;               # number of intervals.
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..nh} >=0, <= L; 
var the {0..nh} >= -pi, <= pi; 
var phi {0..nh} >=0, <= pi;

# The derivatives of the length and the angles.
    
var rho_dot {0..nh};
var the_dot {0..nh};
var phi_dot {0..nh};

# The controls.

var u_rho {0..nh} >= -max_u_rho, <= max_u_rho;
var u_the {0..nh} >= -max_u_the, <= max_u_the;
var u_phi {0..nh} >= -max_u_phi, <= max_u_phi;

# The step and the final time.

var tf;
var step = tf/nh;

# The moments of inertia.

var I_the {i in 0..nh} = ((L-rho[i])^3+rho[i]^3)*(sin(phi[i]))^2/3.0;
var I_phi {i in 0..nh} = ((L-rho[i])^3+rho[i]^3)/3.0;

# The robot arm problem.

minimize time: tf;

subject to rho_eqn {j in 1..nh}:
        rho[j] = rho[j-1] + 0.5*step*(rho_dot[j] + rho_dot[j-1]);

subject to the_eqn {j in 1..nh}:
        the[j] = the[j-1] + 0.5*step*(the_dot[j] + the_dot[j-1]);

subject to phi_eqn {j in 1..nh}:
        phi[j] = phi[j-1] + 0.5*step*(phi_dot[j] + phi_dot[j-1]);

subject to u_rho_eqn {j in 1..nh}:
        rho_dot[j] = rho_dot[j-1] + 0.5*step*(u_rho[j] + u_rho[j-1])/L;

subject to u_the_eqn {j in 1..nh}:
        the_dot[j] = the_dot[j-1] + 
                   0.5*step*(u_the[j]/I_the[j] + u_the[j-1]/I_the[j-1]);

subject to u_phi_eqn {j in 1..nh}:
        phi_dot[j] = phi_dot[j-1] + 
                   0.5*step*(u_phi[j]/I_phi[j] + u_phi[j-1]/I_phi[j-1]);

# Boundary Conditions

subject to rho_0_eqn: rho[0] = 4.5;
subject to the_0_eqn: the[0] = 0;
subject to phi_0_eqn: phi[0] = pi/4;

subject to rho_f_eqn: rho[nh] = 4.5;
subject to the_f_eqn: the[nh] = 2*pi/3;
subject to phi_f_eqn: phi[nh] = pi/4;

subject to rho_dot_0_eqn: rho_dot[0] = 0;
subject to the_dot_0_eqn: the_dot[0] = 0;
subject to phi_dot_0_eqn: phi_dot[0] = 0;

subject to rho_dot_f_eqn: rho_dot[nh] = 0;
subject to the_dot_f_eqn: the_dot[nh] = 0;
subject to phi_dot_f_eqn: phi_dot[nh] = 0;

robotarm.jl

using JuMP, Ipopt

mod = Model(solver=IpoptSolver())

# Parameters
nh = 400    # Number of intervals
tf = 1.0    # Final time
Δt = tf/nh  # Time step
L  = 5      # Length of arm
# Upper bounds on controls
max_u_ρ = 1.0
max_u_θ = 1.0
max_u_ϕ = 1.0

# State: length & angles for arm
@defVar(mod,  0 ≤ ρ[0:nh] ≤ L)
@defVar(mod, -π ≤ θ[0:nh] ≤ π)
@defVar(mod,  0 ≤ ϕ[0:nh] ≤ π)
# State: derivatives
@defVar(mod, ρ′[0:nh])
@defVar(mod, θ′[0:nh])
@defVar(mod, ϕ′[0:nh])
# Controls
@defVar(mod, -max_u_ρ ≤ u_ρ[0:nh] ≤ max_u_ρ)
@defVar(mod, -max_u_θ ≤ u_θ[0:nh] ≤ max_u_θ)
@defVar(mod, -max_u_ϕ ≤ u_ϕ[0:nh] ≤ max_u_ϕ)

# Moments of inertia
@defNLExpr(I_θ[i=0:nh], ((L-ρ[i])^3+ρ[i]^3)*(sin(ϕ[i]))^2/3.0)
@defNLExpr(I_ϕ[i=0:nh], ((L-ρ[i])^3+ρ[i]^3)/3.0)

# Constant objective
@setObjective(mod, Min, tf)

# Trapazoidal rules for motion
for j in 1:nh
    # Acutal from derivatives
    @addNLConstraint(mod, ρ[j] == ρ[j-1] + 0.5*Δt*(ρ′[j]+ρ′[j-1]))
    @addNLConstraint(mod, θ[j] == θ[j-1] + 0.5*Δt*(θ′[j]+θ′[j-1]))
    @addNLConstraint(mod, ϕ[j] == ϕ[j-1] + 0.5*Δt*(ϕ′[j]+ϕ′[j-1]))
    # Derivatives from control
    @addNLConstraint(mod, ρ′[j] == ρ′[j-1] + 0.5*Δt*(
                            u_ρ[j]        + u_ρ[j-1])/L)
    @addNLConstraint(mod, θ′[j] == θ′[j-1] + 0.5*Δt*(
                            u_θ[j]/I_θ[j] + u_θ[j-1]/I_θ[j-1]))
    @addNLConstraint(mod, ϕ′[j] == ϕ′[j-1] + 0.5*Δt*(
                            u_ϕ[j]/I_ϕ[j] + u_ϕ[j-1]/I_ϕ[j-1]))
end

# Boundary conditions
@addConstraint(mod, ρ[0] == 4.5)
@addConstraint(mod, θ[0] == 0)
@addConstraint(mod, ϕ[0] == π/4)
@addConstraint(mod, ρ[nh] == 4.5)
@addConstraint(mod, θ[nh] == 2π/3)
@addConstraint(mod, ϕ[nh] == π/4)

@addConstraint(mod, ρ′[0] == 0)
@addConstraint(mod, θ′[0] == 0)
@addConstraint(mod, ϕ′[0] == 0)
@addConstraint(mod, ρ′[nh] == 0)
@addConstraint(mod, θ′[nh] == 0)
@addConstraint(mod, ϕ′[nh] == 0)

# Initial values
for k in 0:nh
    setValue(ρ[k], 4.5)
    setValue(θ[k], 2π/3*(k/nh)^2)
    setValue(ϕ[k], π/4)
    setValue(ρ′[k], 0.0)
    setValue(θ′[k], 4π/3*(k/nh))
    setValue(ϕ′[k], 0.0)
    setValue(u_ρ[k], 0.0)
    setValue(u_θ[k], 0.0)
    setValue(u_ϕ[k], 0.0)
end

status = solve(mod)