RobotArm

RobotArm

This problem can be found here. although that example is missing initial and final state constraints and limits on x4

Packages that will be used

using NLOptControl

Define the Problem:

n = define(numStates=6,numControls=3,X0=[9/2,0.0,0.0,0.0,pi/4,0.0],XF=[9/2,0.0,2*pi/3,0.0,pi/4,0.0],XL=[NaN,NaN,NaN,0.0,NaN,NaN],XU=[NaN,NaN,NaN,1.0,NaN,NaN],CL=[-1.,-1.,-1.],CU=[1.,1.,1.])

Constants

EP = 2*eps(); # to avoid divide/0
Q = 5

Differential Equations

# expressions
I_t = :((($Q-x1[j])^3+x1[j]^3)/3*sin(x5[j])^2)
I_p = :((($Q-x1[j])^3+x1[j]^3)/3 )

# Diff Eqs
dx = Array{Expr}(6,)
dx[1] = :(x2[j])
dx[2] = :(u1[j]/$Q)
dx[3] = :(x4[j])
dx[4] = :(u2[j]/($I_t+$EP))
dx[5] = :(x6[j])
dx[6] = :(u3[j]/($I_p+$EP))
:(u3[j] / (((5 - x1[j]) ^ 3 + x1[j] ^ 3) / 3 + 4.440892098500626e-16))

Then add the differential equations to the model:

dynamics!(n,dx)

Configure the Problem:

configure!(n;(:finalTimeDV=>true))

Objective Function

@NLobjective(n.ocp.mdl,Min,n.ocp.tf)

Optimize

optimize!(n)

******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
 Ipopt is released as open source code under the Eclipse Public License (EPL).
         For more information visit http://projects.coin-or.org/Ipopt
******************************************************************************

Post Process

allPlots(n)
0 2 4 6 8 3.4 3.6 3.8 4.0 4.2 4.4 4.6 time (s) x1 mpc 0 2 4 6 8 -0.4 -0.2 0.0 0.2 0.4 time (s) x2 mpc 0 2 4 6 8 0.0 0.5 1.0 1.5 2.0 time (s) x3 mpc 0 2 4 6 8 0.00 0.25 0.50 0.75 1.00 time (s) x4 max min mpc 0 2 4 6 8 0.55 0.60 0.65 0.70 0.75 0.80 time (s) x5 mpc 0 2 4 6 8 -0.10 -0.05 0.00 0.05 0.10 time (s) x6 mpc 0 2 4 6 8 -1.0 -0.5 0.0 0.5 1.0 time (s) u1 max min mpc 0 2 4 6 8 -1.0 -0.5 0.0 0.5 1.0 time (s) u2 max min mpc 0 2 4 6 8 -1.0 -0.5 0.0 0.5 1.0 time (s) u3 max min mpc