Robot

Description of the problem

The robot arm motion problem is a benchmark in time-optimal control. It models a robotic arm moving between two points in space, represented as a rigid bar of total length $L$ pivoting at the origin of a spherical coordinate system. The system includes six state variables: the radial position $\rho(t)$ and its velocity $d\rho(t)$, the horizontal angle $\theta(t)$ and its angular velocity $d\theta(t)$, and the vertical angle $\phi(t)$ and its angular velocity $d\phi(t)$. The control variables are $u_\rho(t)$, $u_\theta(t)$, and $u_\phi(t)$, representing the forces or torques applied along the radial and angular directions. The objective is to minimise the total transfer time while satisfying state and control constraints BOCOP Robot Arm Example.

Mathematical formulation

The problem can be stated as

\[\begin{aligned} \min_{\rho, d\rho, \theta, d\theta, \phi, d\phi, u_\rho, u_\theta, u_\phi, T} \quad & J = T \\[0.5em] \text{s.t.} \quad & \dot{\rho}(t) = d\rho(t), \quad \dot{\theta}(t) = d\theta(t), \quad \dot{\phi}(t) = d\phi(t), \\[0.5em] & \dot{d\rho}(t) = \frac{u_\rho(t)}{L}, \quad \dot{d\theta}(t) = \frac{3 u_\theta(t)}{I_\theta(t)}, \quad \dot{d\phi}(t) = \frac{3 u_\phi(t)}{I_\phi(t)}, \\[0.5em] & I_\theta(t) = ((L-\rho(t))^3 + \rho(t)^3) \sin^2(\phi(t)), \quad I_\phi(t) = (L-\rho(t))^3 + \rho(t)^3, \\[0.5em] & 0 \le \rho(t) \le L, \quad -\pi \le \theta(t) \le \pi, \quad 0 \le \phi(t) \le \pi, \\[0.5em] & |u_\rho(t)| \le 1, \quad |u_\theta(t)| \le 1, \quad |u_\phi(t)| \le 1, \\[0.5em] & \rho(0) = \rho(T) = 4.5, \quad \theta(0) = 0, \quad \theta(T) = \frac{2\pi}{3}, \quad \phi(0) = \phi(T) = \frac{\pi}{4}, \\[0.5em] & d\rho(0) = d\rho(T) = 0, \quad d\theta(0) = d\theta(T) = 0, \quad d\phi(0) = d\phi(T) = 0, \\[0.5em] & T \ge 0.1. \end{aligned}\]

The horizon is free, as the total transfer time $T$ is optimised.

Parameter values

Qualitative behaviour

The optimal solution exploits the degrees of freedom of the spherical coordinate system to minimise the motion time. Control inputs $u_\rho$, $u_\theta$, and $u_\phi$ typically follow bang–bang profiles, alternating between maximum and minimum values to reach the target efficiently. The dynamics are nonlinear due to the dependence of the moments of inertia $I_\theta$ and $I_\phi$ on the radial and angular positions, which affects the acceleration of the arm.

Characteristics

• Nonlinear, second-order dynamics in spherical coordinates,
• Free final time with state and control constraints,
• Bang–bang control structure in the time-optimal solution,
• Serves as a benchmark for direct transcription and NLP solvers in robotic motion planning.

References

• Mössner-Beigel, M. (1989). Thesis, Heidelberg University. Introduces the time-optimal robotic arm problem and its theoretical formulation.

• Vanderbei, R. J. (2001). Case studies in trajectory optimization: Trains, Planes, and Other Pastimes. Provides numerical strategies for robotic motion planning and discusses control substitution techniques for time-optimal trajectories.

• BOCOP examples: Robot arm problem. BOCOP Robot Arm Example Demonstrates the practical implementation using direct transcription and NLP solvers for robotic motion problems.

Numerical set-up

In this section, we prepare the numerical environment required to study the problem. We begin by importing the relevant Julia packages and then initialise the data frames that will store the results of our simulations and computations. These structures provide the foundation for solving the problem and for comparing the different solution strategies in a consistent way.

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
using Printf                    # to print

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[],
)

Metadata

The default number of time steps is:

metadata(:robot)[:grid_size]

250

The default values of the parameters are:

metadata(:robot)[:parameters]

Parameter = Value
------------------
    t0 =  0.0000e+00
     L =  5.0000e+00
  uρ_l = -1.0000e+00
  uρ_u =  1.0000e+00
  uθ_l = -1.0000e+00
  uθ_u =  1.0000e+00
  uϕ_l = -1.0000e+00
  uϕ_u =  1.0000e+00
  ρ_t0 =  4.5000e+00
  θ_t0 =  0.0000e+00
  ϕ_t0 =  7.8540e-01
 dρ_t0 =  0.0000e+00
 dθ_t0 =  0.0000e+00
 dϕ_t0 =  0.0000e+00
  ρ_tf =  4.5000e+00
  θ_tf =  2.0944e+00
  ϕ_tf =  7.8540e-01
 dρ_tf =  0.0000e+00
 dθ_tf =  0.0000e+00
 dϕ_tf =  0.0000e+00
   ρ_l =  0.0000e+00
   θ_l = -3.1416e+00
   θ_u =  3.1416e+00
   ϕ_l =  0.0000e+00
   ϕ_u =  3.1416e+00
  tf_l =  1.0000e-01

Initial guess

Before solving the problem, it is often useful to inspect the initial guess (sometimes called the first iterate). This guess is obtained by running the NLP solver with max_iter = 0, which evaluates the problem formulation without performing any optimisation steps.

We plot the resulting trajectories for both the OptimalControl and JuMP models. Since both backends represent the same mathematical problem, their initial guesses should coincide, providing a useful consistency check before moving on to the optimised solution.

function plot_initial_guess(problem)

    # -----------------------------
    # Build OptimalControl problem
    # -----------------------------
    docp = eval(problem)(OptimalControlBackend())
    nlp_oc = nlp_model(docp)
    ocp_oc = ocp_model(docp)

    # Solve NLP with zero iterations (initial guess)
    nlp_oc_sol = NLPModelsIpopt.ipopt(nlp_oc; max_iter=0)

    # Build OptimalControl solution
    ocp_sol = build_ocp_solution(docp, nlp_oc_sol)

    # get dimensions
    n = state_dimension(ocp_oc)
    m = control_dimension(ocp_oc)

    # -----------------------------
    # Plot 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,
    )

    # Hide legend for additional state plots
    for i in 2:n
        plot!(plt[i]; legend=:none)
    end

    # -----------------------------
    # Build JuMP model
    # -----------------------------
    nlp_jp = eval(problem)(JuMPBackend())

    # Solve NLP with zero iterations (initial guess)
    set_optimizer(nlp_jp, Ipopt.Optimizer)
    set_optimizer_attribute(nlp_jp, "max_iter", 0)
    optimize!(nlp_jp)

    # Extract trajectories
    t_grid = time_grid(nlp_jp)
    x_fun = state(nlp_jp)
    u_fun = control(nlp_jp)
    p_fun = costate(nlp_jp)

    # -----------------------------
    # Plot JuMP solution on top
    # -----------------------------
    # States
    for i in 1:n
        label = i == 1 ? "JuMP" : :none
        plot!(plt[i], t_grid, t -> x_fun(t)[i]; color=2, linestyle=:dash, label=label)
    end

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

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

    return plt
end

plot_initial_guess(:robot)

Solving the problem

To solve an optimal control problem, we can rely on two complementary formulations: the OptimalControl backend, which works directly with the discretised control problem, and the JuMP backend, which leverages JuMP’s flexible modelling framework.

Both approaches generate equivalent NLPs that can be solved with Ipopt, and comparing them ensures consistency between the two formulations.

Before solving, we can inspect the discretisation details of the problem. The table below reports the number of grid points, decision variables, and constraints associated with the chosen formulation.

push!(data_pb,(
    Problem=:robot,
    Grid_Size=metadata(:robot)[:grid_size],
    Variables=get_nvar(nlp_model(robot(OptimalControlBackend()))),
    Constraints=get_ncon(nlp_model(robot(OptimalControlBackend()))),
))

OptimalControl model

We first solve the problem using the OptimalControl backend. The process begins by importing the problem definition and constructing the associated nonlinear programming (NLP) model. This NLP is then passed to the Ipopt solver, with standard options for tolerance and barrier parameter strategy.

# 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.1411253337358414e+00    9.1411253337358414e+00
Dual infeasibility......:   7.9424516182674543e-09    7.9424516182674543e-09
Constraint violation....:   1.1444456493592270e-11    1.1444456493592270e-11
Variable bound violation:   9.9006365328335733e-09    9.9006365328335733e-09
Complementarity.........:   1.6907072725294024e-09    1.6907072725294024e-09
Overall NLP error.......:   7.9424516182674543e-09    7.9424516182674543e-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.700

EXIT: Optimal Solution Found.

JuMP model

We now repeat the procedure using the JuMP backend. Here, the problem is reformulated as a JuMP model, which offers a flexible and widely used framework for nonlinear optimisation in Julia. The solver settings are chosen to mirror those used previously, so that the results can be compared on an equal footing.

# import model
nlp_jp = robot(JuMPBackend())

# solve with Ipopt
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.9424516286757951e-09    7.9424516286757951e-09
Constraint violation....:   1.1444446952613152e-11    1.1444446952613152e-11
Variable bound violation:   9.9006365328335733e-09    9.9006365328335733e-09
Complementarity.........:   1.6907072727031558e-09    1.6907072727031558e-09
Overall NLP error.......:   7.9424516286757951e-09    7.9424516286757951e-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.179

EXIT: Optimal Solution Found.

Numerical comparisons

In this section, we examine the results of the problem resolutions. We extract the solver status (flag), the number of iterations, and the objective value for each model. This provides a first overview of how each approach performs and sets the stage for a more detailed comparison of the solution trajectories.

# 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 solutions obtained from the OptimalControl and JuMP models by examining the number of iterations required for convergence, the $L^2$-norms of the differences in states, controls, and additional variables, and the corresponding objective values. Both absolute and relative errors are reported, providing a clear quantitative measure of the agreement between the two approaches.

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 print_numerical_comparisons(problem, docp, nlp_oc_sol, nlp_jp)

    # get relevant data from OptimalControl model
    ocp_sol = build_ocp_solution(docp, nlp_oc_sol)
    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(nlp_jp)
    x_jp = state(nlp_jp).(t_jp)
    u_jp = control(nlp_jp).(t_jp)
    o_jp = objective(nlp_jp)
    v_jp = variable(nlp_jp)
    i_jp = iterations(nlp_jp)

    x_vars = state_components(nlp_jp)
    u_vars = control_components(nlp_jp)
    v_vars = variable_components(nlp_jp)

    println("┌─ ", string(problem))
    println("│")
    println("├─  Number of Iterations")
    @printf("│     OptimalControl : %d   JuMP : %d\n", i_oc, i_jp)

    # States
    println("├─  States (L2 Norms)")
    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_ae = L2_norm(t_oc, xi_oc - xi_jp)
        L2_re = L2_ae / (0.5 * (L2_norm(t_oc, xi_oc) + L2_norm(t_oc, xi_jp)))
        @printf("│     %-6s Abs: %.3e   Rel: %.3e\n", x_vars[i], L2_ae, L2_re)
    end

    # Controls
    println("├─  Controls (L2 Norms)")
    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_ae = L2_norm(t_oc, ui_oc - ui_jp)
        L2_re = L2_ae / (0.5 * (L2_norm(t_oc, ui_oc) + L2_norm(t_oc, ui_jp)))
        @printf("│     %-6s Abs: %.3e   Rel: %.3e\n", u_vars[i], L2_ae, L2_re)
    end

    # Variables
    if !isnothing(v_vars)
        println("├─  Variables")
        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)))
            @printf("│     %-6s Abs: %.3e   Rel: %.3e\n", v_vars[i], vi_ae, vi_re)
        end
    end

    # Objective
    o_ae = abs(o_oc - o_jp)
    o_re = o_ae / (0.5 * (abs(o_oc) + abs(o_jp)))
    println("├─  Objective")
    @printf("│            Abs: %.3e   Rel: %.3e\n", o_ae, o_re)
    println("└─")
    return nothing
end

print_numerical_comparisons(:robot, docp, nlp_oc_sol, nlp_jp)

┌─ robot
│
├─  Number of Iterations
│     OptimalControl : 19   JuMP : 19
├─  States (L2 Norms)
│     ρ      Abs: 4.513e-15   Rel: 3.736e-16
│     dρ     Abs: 9.658e-16   Rel: 1.211e-15
│     θ      Abs: 8.985e-16   Rel: 2.248e-16
│     dθ     Abs: 5.208e-16   Rel: 5.951e-16
│     ϕ      Abs: 4.718e-16   Rel: 2.287e-16
│     dϕ     Abs: 2.235e-16   Rel: 1.194e-15
├─  Controls (L2 Norms)
│     uρ     Abs: 1.276e-14   Rel: 4.221e-15
│     uθ     Abs: 9.370e-16   Rel: 3.105e-16
│     uϕ     Abs: 1.941e-14   Rel: 6.422e-15
├─  Variables
│     tf     Abs: 1.776e-15   Rel: 1.943e-16
├─  Objective
│            Abs: 1.776e-15   Rel: 1.943e-16
└─

Plotting the solutions

In this section, we visualise the trajectories of the states, costates, and controls obtained from both the OptimalControl and JuMP solutions. The plots provide an intuitive way to compare the two approaches and to observe how the constraints and the optimal control influence the system dynamics.

For each variable, the OptimalControl solution is shown in solid lines, while the JuMP solution is overlaid using dashed lines. Since both models represent the same mathematical problem, their trajectories should closely coincide, highlighting the consistency between the two formulations.

# 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)
m = control_dimension(ocp_sol)

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

# from JuMP solution
t = time_grid(nlp_jp)     # t0, ..., tN = tf
x = state(nlp_jp)         # function of time
u = control(nlp_jp)       # function of time
p = costate(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