Robot

This problem models the time-optimal motion of a robot arm moving between two points, following the formulation of Mössner-Beigel (PhD thesis, Heidelberg University) and the implementation described by Vanderbei (2001). The arm is represented as a rigid bar of total length $L$, pivoting at the origin of a spherical coordinate system.

System Description

• The arm protrudes a distance $\rho$ from the origin in one direction, and $L - \rho$ in the opposite direction.
• The orientation of the arm is described by two angles:

  • \[\theta\]

    : horizontal angle from the plane, with bounds $|\theta| \leq \pi$

  • \[\varphi\]

    : vertical angle, with bounds $0 \leq \varphi \leq \pi$

The state variables are:

\[x = (\rho, \dot{\rho}, \theta, \dot{\theta}, \varphi, \dot{\varphi})\]

The control inputs are:

\[u = (u_{\rho}, u_{\theta}, u_{\varphi})\]

Dynamics

The continuous-time dynamics are governed by the second-order system:

\[L \ddot{\rho} = u_{\rho}\]

\[I_{\theta} \ddot{\theta} = u_{\theta}\]

\[I_{\varphi} \ddot{\varphi} = u_{\varphi}\]

where the moments of inertia are defined as:

\[I_{\theta} = \big( (L - \rho)^3 + \rho^3 \big) \sin^2(\varphi)\]

\[I_{\varphi} = (L - \rho)^3 + \rho^3\]

In first-order form, with states $(\rho, \dot{\rho}, \theta, \dot{\theta}, \varphi, \dot{\varphi})$, the dynamics become:

\[\dot{\rho} = \dot{\rho}, \quad \ddot{\rho} = \frac{u_{\rho}}{L}\]

\[\dot{\theta} = \dot{\theta}, \quad \ddot{\theta} = \frac{3u_{\theta}}{I_{\theta}}\]

\[\dot{\varphi} = \dot{\varphi}, \quad \ddot{\varphi} = \frac{3u_{\varphi}}{I_{\varphi}}\]

Constraints

• State constraints:

\[0 \leq \rho(t) \leq L\]

\[-\pi \leq \theta(t) \leq \pi\]

\[0 \leq \varphi(t) \leq \pi\]

• Control constraints:

\[|u_{\rho}(t)| \leq 1, \quad |u_{\theta}(t)| \leq 1, \quad |u_{\varphi}(t)| \leq 1\]

• Initial conditions:

\[\rho(0) = 4.5, \quad \theta(0) = 0, \quad \varphi(0) = \pi/4\]

\[\dot{\rho}(0) = \dot{\theta}(0) = \dot{\varphi}(0) = 0\]

• Final conditions:

\[\rho(T) = 4.5, \quad \theta(T) = \tfrac{2\pi}{3}, \quad \varphi(T) = \pi/4\]

\[\dot{\rho}(T) = \dot{\theta}(T) = \dot{\varphi}(T) = 0\]

Objective

The objective is to minimise the transfer time $T$:

\[J = T \to \min\]

Remarks

This model is simplified, as it ignores the non-inertial nature of the spherical coordinate frame (i.e., Coriolis and centrifugal forces are not included). Vanderbei’s implementation eliminates the controls $u$ by substitution, transforming the equations of motion into inequality constraints on accelerations.

References

• Mössner-Beigel, M. (1989). Thesis, Heidelberg University.
• Vanderbei, R. J. (2001). Case studies in trajectory optimization: Trains, Planes, and Other Pastimes.
• More, J., Garbow, B., Hillstrom, K., & Watson, L. (2001). COPS: Constrained Optimization Problem Set (COPS3). Argonne National Laboratory. Retrieved from https://www.mcs.anl.gov/~more/cops/cops3.pdf

Packages

Import all necessary packages and define DataFrames to store information about the problem and resolution results.

using OptimalControlProblems    # to access the Beam model
using OptimalControl            # to import the OptimalControl model
using NLPModelsIpopt            # to solve the model with Ipopt
import DataFrames: DataFrame    # to store data
using NLPModels                 # to retrieve data from the NLP solution
using Plots                     # to plot the trajectories
using Plots.PlotMeasures        # for leftmargin, bottommargin
using JuMP                      # to import the JuMP model
using Ipopt                     # to solve the JuMP model with Ipopt

data_pb = DataFrame(            # to store data about the problem
    Problem=Symbol[],
    Grid_Size=Int[],
    Variables=Int[],
    Constraints=Int[],
)

data_re = DataFrame(            # to store data about the resolutions
    Model=Symbol[],
    Flag=Any[],
    Iterations=Int[],
    Objective=Float64[],
)

Initial guess

The initial guess (or first iterate) can be visualised by running the solver with max_iter=0. Here is the initial guess.

function plot_initial_guess(problem)

    # dimensions
    x_vars = metadata[problem][:state_name]
    u_vars = metadata[problem][:control_name]
    n = length(x_vars) # number of states
    m = length(u_vars) # number of controls

    # import OptimalControl model
    docp = eval(problem)(OptimalControlBackend())
    nlp_oc = nlp_model(docp)

    # solve
    nlp_oc_sol = NLPModelsIpopt.ipopt(nlp_oc; max_iter=0)

    # build an optimal control solution
    ocp_sol = build_ocp_solution(docp, nlp_oc_sol)

    # plot the OptimalControl solution
    plt = plot(
        ocp_sol;
        state_style=(color=1,),
        costate_style=(color=1, legend=:none),
        control_style=(color=1, legend=:none),
        path_style=(color=1, legend=:none),
        dual_style=(color=1, legend=:none),
        size=(816, 220*(n+m)),
        label="OptimalControl",
        leftmargin=20mm,
    )
    for i in 2:n
        plot!(plt[i]; legend=:none)
    end

    # import JuMP model
    nlp_jp = eval(problem)(JuMPBackend())

    # solve
    set_optimizer(nlp_jp, Ipopt.Optimizer)
    set_optimizer_attribute(nlp_jp, "max_iter", 0)
    optimize!(nlp_jp)

    # plot
    t = time_grid(problem, nlp_jp)     # t0, ..., tN = tf
    x = state(problem, nlp_jp)         # function of time
    u = control(problem, nlp_jp)       # function of time
    p = costate(problem, nlp_jp)       # function of time

    for i in 1:n # state
        label = i == 1 ? "JuMP" : :none
        plot!(plt[i], t, t -> x(t)[i]; color=2, linestyle=:dash, label=label)
    end

    for i in 1:n # costate
        plot!(plt[n+i], t, t -> -p(t)[i]; color=2, linestyle=:dash, label=:none)
    end

    for i in 1:m # control
        plot!(plt[2n+i], t, t -> u(t)[i]; color=2, linestyle=:dash, label=:none)
    end

    return plt
end

plot_initial_guess(:robot)

Solve the problem

OptimalControl model

Import the OptimalControl model and solve it.

# import DOCP model
docp = robot(OptimalControlBackend())

# get NLP model
nlp_oc = nlp_model(docp)

# solve
nlp_oc_sol = NLPModelsIpopt.ipopt(
    nlp_oc;
    print_level=4,
    tol=1e-8,
    mu_strategy="adaptive",
    sb="yes",
)

Total number of variables............................:     2260
                     variables with only lower bounds:        1
                variables with lower and upper bounds:     1506
                     variables with only upper bounds:        0
Total number of equality constraints.................:     1512
Total number of inequality constraints...............:        0
        inequality constraints with only lower bounds:        0
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:        0


Number of Iterations....: 19

                                   (scaled)                 (unscaled)
Objective...............:   9.1411253337358431e+00    9.1411253337358431e+00
Dual infeasibility......:   7.9424516169664117e-09    7.9424516169664117e-09
Constraint violation....:   1.1444345471289807e-11    1.1444345471289807e-11
Variable bound violation:   9.9006365328335733e-09    9.9006365328335733e-09
Complementarity.........:   1.6907072724884652e-09    1.6907072724884652e-09
Overall NLP error.......:   7.9424516169664117e-09    7.9424516169664117e-09


Number of objective function evaluations             = 20
Number of objective gradient evaluations             = 20
Number of equality constraint evaluations            = 20
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 20
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 19
Total seconds in IPOPT                               = 0.646

EXIT: Optimal Solution Found.

The problem has the following numbers of steps, variables and constraints.

push!(data_pb,
    (
        Problem=:robot,
        Grid_Size=metadata[:robot][:N],
        Variables=get_nvar(nlp_oc),
        Constraints=get_ncon(nlp_oc),
    )
)

JuMP model

Import the JuMP model and solve it.

# import model
nlp_jp = robot(JuMPBackend())

# solve
set_optimizer(nlp_jp, Ipopt.Optimizer)
set_optimizer_attribute(nlp_jp, "print_level", 4)
set_optimizer_attribute(nlp_jp, "tol", 1e-8)
set_optimizer_attribute(nlp_jp, "mu_strategy", "adaptive")
set_optimizer_attribute(nlp_jp, "linear_solver", "mumps")
set_optimizer_attribute(nlp_jp, "sb", "yes")
optimize!(nlp_jp)

Total number of variables............................:     2260
                     variables with only lower bounds:        1
                variables with lower and upper bounds:     1506
                     variables with only upper bounds:        0
Total number of equality constraints.................:     1512
Total number of inequality constraints...............:        0
        inequality constraints with only lower bounds:        0
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:        0


Number of Iterations....: 19

                                   (scaled)                 (unscaled)
Objective...............:   9.1411253337358431e+00    9.1411253337358431e+00
Dual infeasibility......:   7.9424516360483699e-09    7.9424516360483699e-09
Constraint violation....:   1.1444380599440196e-11    1.1444380599440196e-11
Variable bound violation:   9.9006365328335733e-09    9.9006365328335733e-09
Complementarity.........:   1.6907072723147017e-09    1.6907072723147017e-09
Overall NLP error.......:   7.9424516360483699e-09    7.9424516360483699e-09


Number of objective function evaluations             = 20
Number of objective gradient evaluations             = 20
Number of equality constraint evaluations            = 20
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 20
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 19
Total seconds in IPOPT                               = 0.123

EXIT: Optimal Solution Found.

Numerical comparisons

Let's get the flag, the number of iterations and the objective value from the resolutions.

# from OptimalControl model
push!(data_re,
    (
        Model=:OptimalControl,
        Flag=nlp_oc_sol.status,
        Iterations=nlp_oc_sol.iter,
        Objective=nlp_oc_sol.objective,
    )
)

# from JuMP model
push!(data_re,
    (
        Model=:JuMP,
        Flag=termination_status(nlp_jp),
        Iterations=barrier_iterations(nlp_jp),
        Objective=objective_value(nlp_jp),
    )
)

We compare the OptimalControl and JuMP solutions in terms of the number of iterations, the $L^2$-norm of the differences in the state, control, and variable, as well as the objective values. Both absolute and relative errors are reported.

function L2_norm(T, X)
    # T and X are supposed to be one dimensional
    s = 0.0
    for i in 1:(length(T) - 1)
        s += 0.5 * (X[i]^2 + X[i + 1]^2) * (T[i + 1]-T[i])
    end
    return √(s)
end

function numerical_comparison(problem, docp, nlp_oc_sol, nlp_jp)

    # get relevant data from OptimalControl model
    ocp_sol = build_ocp_solution(docp, nlp_oc_sol) # build an ocp solution
    t_oc = time_grid(ocp_sol)
    x_oc = state(ocp_sol).(t_oc)
    u_oc = control(ocp_sol).(t_oc)
    v_oc = variable(ocp_sol)
    o_oc = objective(ocp_sol)
    i_oc = iterations(ocp_sol)

    # get relevant data from JuMP model
    t_jp = time_grid(problem, nlp_jp)
    x_jp = state(problem, nlp_jp).(t_jp)
    u_jp = control(problem, nlp_jp).(t_jp)
    o_jp = objective(problem, nlp_jp)
    v_jp = variable(problem, nlp_jp)
    i_jp = iterations(problem, nlp_jp)

    x_vars = metadata[problem][:state_name]
    u_vars = metadata[problem][:control_name]
    v_vars = metadata[problem][:variable_name]

    println("┌─ ", string(problem))
    println("│")

    # number of iterations
    println("├─  Number of iterations")
    println("│")
    println("│     OptimalControl : ", i_oc)
    println("│     JuMP           : ", i_jp)
    println("│")

    # state
    for i in eachindex(x_vars)
        xi_oc = [x_oc[k][i] for k in eachindex(t_oc)]
        xi_jp = [x_jp[k][i] for k in eachindex(t_jp)]
        L2_oc = L2_norm(t_oc, xi_oc)
        L2_jp = L2_norm(t_oc, xi_jp)
        L2_ae = L2_norm(t_oc, xi_oc-xi_jp)
        L2_re = L2_ae/(0.5*(L2_oc + L2_jp))

        println("├─  State $(x_vars[i]) (L2 norm)")
        println("│")
        #println("│     OptimalControl : ", L2_oc)
        #println("│     JuMP           : ", L2_jp)
        println("│     Absolute error : ", L2_ae)
        println("│     Relative error : ", L2_re)
        println("│")
    end

    # control
    for i in eachindex(u_vars)
        ui_oc = [u_oc[k][i] for k in eachindex(t_oc)]
        ui_jp = [u_jp[k][i] for k in eachindex(t_jp)]
        L2_oc = L2_norm(t_oc, ui_oc)
        L2_jp = L2_norm(t_oc, ui_jp)
        L2_ae = L2_norm(t_oc, ui_oc-ui_jp)
        L2_re = L2_ae/(0.5*(L2_oc + L2_jp))

        println("├─  Control $(u_vars[i]) (L2 norm)")
        println("│")
        #println("│     OptimalControl : ", L2_oc)
        #println("│     JuMP           : ", L2_jp)
        println("│     Absolute error : ", L2_ae)
        println("│     Relative error : ", L2_re)
        println("│")
    end

    # variable
    if !isnothing(v_vars)
        for i in eachindex(v_vars)
            vi_oc = v_oc[i]
            vi_jp = v_jp[i]
            vi_ae = abs(vi_oc-vi_jp)
            vi_re = vi_ae/(0.5*(abs(vi_oc) + abs(vi_jp)))

            println("├─  Variable $(v_vars[i])")
            println("│")
            #println("│     OptimalControl : ", vi_oc)
            #println("│     JuMP           : ", vi_jp)
            println("│     Absolute error : ", vi_ae)
            println("│     Relative error : ", vi_re)
            println("│")
        end
    end

    # objective
    o_ae = abs(o_oc-o_jp)
    o_re = o_ae/(0.5*(abs(o_oc) + abs(o_jp)))

    println("├─  objective")
    println("│")
    #println("│     OptimalControl : ", o_oc)
    #println("│     JuMP           : ", o_jp)
    println("│     Absolute error : ", o_ae)
    println("│     Relative error : ", o_re)
    println("│")
    println("└─")

    return nothing
end

numerical_comparison(:robot, docp, nlp_oc_sol, nlp_jp)

┌─ robot
│
├─  Number of iterations
│
│     OptimalControl : 19
│     JuMP           : 19
│
├─  State ρ (L2 norm)
│
│     Absolute error : 4.852232935874855e-15
│     Relative error : 4.0161868435980575e-16
│
├─  State dρ (L2 norm)
│
│     Absolute error : 2.165180820517422e-15
│     Relative error : 2.7139337118023713e-15
│
├─  State θ (L2 norm)
│
│     Absolute error : 1.220468383424293e-15
│     Relative error : 3.053250032884345e-16
│
├─  State dθ (L2 norm)
│
│     Absolute error : 9.428240286950934e-16
│     Relative error : 1.0772475482805366e-15
│
├─  State ϕ (L2 norm)
│
│     Absolute error : 7.338782385631124e-16
│     Relative error : 3.556388618134625e-16
│
├─  State dϕ (L2 norm)
│
│     Absolute error : 2.2400515536757674e-16
│     Relative error : 1.1965415103981847e-15
│
├─  Control uρ (L2 norm)
│
│     Absolute error : 3.4650423998647244e-14
│     Relative error : 1.1460638876849463e-14
│
├─  Control uθ (L2 norm)
│
│     Absolute error : 1.8676006958939164e-14
│     Relative error : 6.189485346790462e-15
│
├─  Control uϕ (L2 norm)
│
│     Absolute error : 2.195648078706063e-14
│     Relative error : 7.265448521101139e-15
│
├─  Variable tf
│
│     Absolute error : 0.0
│     Relative error : 0.0
│
├─  objective
│
│     Absolute error : 0.0
│     Relative error : 0.0
│
└─

Plot the solutions

Visualise states, costates, and controls from the OptimalControl and JuMP solutions:

# build an ocp solution to use the plot from OptimalControl package
ocp_sol = build_ocp_solution(docp, nlp_oc_sol)

# dimensions
n = state_dimension(ocp_sol)   # or length(metadata[:robot][:state_name])
m = control_dimension(ocp_sol) # or length(metadata[:robot][:control_name])

# from OptimalControl solution
plt = plot(
    ocp_sol;
    state_style=(color=1,),
    costate_style=(color=1, legend=:none),
    control_style=(color=1, legend=:none),
    path_style=(color=1, legend=:none),
    dual_style=(color=1, legend=:none),
    size=(816, 240*(n+m)),
    label="OptimalControl",
    leftmargin=20mm,
)
for i in 2:n
    plot!(plt[i]; legend=:none)
end

# from JuMP solution
t = time_grid(:robot, nlp_jp)     # t0, ..., tN = tf
x = state(:robot, nlp_jp)         # function of time
u = control(:robot, nlp_jp)       # function of time
p = costate(:robot, nlp_jp)       # function of time

for i in 1:n # state
    label = i == 1 ? "JuMP" : :none
    plot!(plt[i], t, t -> x(t)[i]; color=2, linestyle=:dash, label=label)
end

for i in 1:n # costate
    plot!(plt[n+i], t, t -> -p(t)[i]; color=2, linestyle=:dash, label=:none)
end

for i in 1:m # control
    plot!(plt[2n+i], t, t -> u(t)[i]; color=2, linestyle=:dash, label=:none)
end