Complete Example: Time-Optimal Bilinear Control
This example demonstrates solving a time-optimal trajectory optimization problem with:
- Multiple control inputs with bounds
- Free time steps (variable Δt)
- Combined objective (control effort + minimum time)
using DirectTrajOpt
using NamedTrajectories
using LinearAlgebra
using CairoMakieProblem Setup
System: 3D oscillator with 2 control inputs
\[\dot{x} = (G_0 + u_1 G_1 + u_2 G_2) x\]
Goal: Drive from [1, 0, 0] to [0, 0, 1] minimizing ∫ ||u||² dt + w·T
Constraints:-1 ≤ u ≤ 1, 0.05 ≤ Δt ≤ 0.3
Define System Dynamics
G_drift = [
0.0 1.0 0.0;
-1.0 0.0 0.0;
0.0 0.0 -0.1
]
G_drives = [
[
1.0 0.0 0.0;
0.0 0.0 0.0;
0.0 0.0 0.0
],
[
0.0 0.0 0.0;
0.0 0.0 1.0;
0.0 1.0 0.0
],
]
G = u -> G_drift + sum(u .* G_drives)#2 (generic function with 1 method)Create Trajectory
N = 50
x_init = [1.0, 0.0, 0.0]
x_goal = [0.0, 0.0, 1.0]
x_guess = hcat([x_init + (x_goal - x_init) * (k/(N-1)) for k = 0:(N-1)]...)
traj = NamedTrajectory(
(x = x_guess, u = 0.1 * randn(2, N), Δt = fill(0.15, N));
timestep = :Δt,
controls = (:u, :Δt),
initial = (x = x_init,),
final = (x = x_goal,),
bounds = (u = 1.0, Δt = (0.05, 0.3)),
)N = 50, (x = 1:3, u = 4:5, → Δt = 6:6)Build and Solve Problem
integrator = BilinearIntegrator(G, :x, :u, traj)
obj = (QuadraticRegularizer(:u, traj, 1.0) + 0.5 * MinimumTimeObjective(traj, 1.0))
prob = DirectTrajOptProblem(traj, obj, integrator)
solve!(prob; max_iter = 50) initializing optimizer...
applying constraint: initial value of x
applying constraint: final value of x
applying constraint: bounds on u
applying constraint: bounds on Δt
******************************************************************************
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 https://github.com/coin-or/Ipopt
******************************************************************************
This is Ipopt version 3.14.19, running with linear solver MUMPS 5.8.2.
Number of nonzeros in equality constraint Jacobian...: 1746
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 2748
Total number of variables............................: 294
variables with only lower bounds: 0
variables with lower and upper bounds: 150
variables with only upper bounds: 0
Total number of equality constraints.................: 147
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
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 3.6855541e+00 1.52e-01 1.03e-01 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 2.7394627e+00 1.33e-01 5.70e-01 -4.0 1.13e+00 - 1.52e-01 1.20e-01f 1
2 2.2861122e+00 1.21e-01 4.72e+00 -1.9 2.92e+00 - 2.79e-01 9.27e-02h 1
3 2.1966076e+00 1.16e-01 4.91e+00 -0.4 6.29e+00 0.0 1.40e-01 4.30e-02f 1
4 2.0399481e+00 1.05e-01 2.16e+01 -0.4 1.98e+00 0.4 6.60e-01 9.60e-02h 1
5 2.0227521e+00 1.02e-01 2.12e+01 -1.1 5.19e+00 - 8.66e-02 2.34e-02h 1
6 2.0460164e+00 9.62e-02 2.20e+01 -0.1 1.03e+01 - 1.66e-01 5.94e-02h 1
7 2.0445751e+00 9.41e-02 3.80e+01 -0.5 2.06e+00 - 3.49e-01 2.10e-02h 1
8 2.3883447e+00 6.32e-02 8.07e+01 -0.3 3.33e+00 - 1.00e+00 3.53e-01h 1
9 2.2857645e+00 5.99e-02 2.51e+03 0.7 1.77e+00 1.8 9.20e-01 5.41e-02h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 2.4501975e+00 5.54e-02 2.10e+03 -0.2 2.40e+01 - 1.79e-02 5.19e-02h 1
11 2.5074963e+00 5.43e-02 2.73e+04 2.0 1.80e+01 2.2 9.14e-01 1.74e-02f 1
12 2.7703120e+00 5.08e-02 2.22e+04 2.3 5.51e+00 - 2.88e-01 8.70e-02f 1
13 3.1736844e+00 8.11e-02 1.17e+04 2.4 3.81e+00 - 3.97e-01 3.06e-01f 1
14 3.2249081e+00 3.70e-02 4.47e+03 2.0 1.26e+00 - 6.10e-01 6.35e-01f 1
15 3.2680410e+00 8.49e-03 1.91e+02 1.5 2.19e-01 - 9.82e-01 1.00e+00f 1
16 3.2858108e+00 5.35e-05 4.32e+00 -0.5 3.07e-02 - 9.91e-01 1.00e+00f 1
17 3.2841874e+00 9.32e-04 4.07e-01 -1.4 8.75e-02 - 9.93e-01 1.00e+00f 1
18 2.9176122e+00 7.28e-02 1.12e-01 -1.9 5.24e-01 - 9.91e-01 1.00e+00f 1
19 2.5546894e+00 2.79e-02 1.60e-01 -2.6 6.53e-01 - 7.73e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 2.4225987e+00 8.01e-03 3.49e-02 -2.7 4.98e-01 - 9.96e-01 1.00e+00h 1
21 2.4171246e+00 2.86e-04 1.46e+00 -3.4 2.88e-02 1.7 9.82e-01 1.00e+00h 1
22 2.4161827e+00 3.12e-07 4.18e-02 -4.0 2.48e-03 1.2 1.00e+00 1.00e+00h 1
23 2.4126833e+00 2.94e-06 4.15e-02 -4.0 7.39e-03 0.7 1.00e+00 1.00e+00h 1
24 2.4026575e+00 2.36e-05 3.86e-02 -4.0 2.06e-02 0.3 1.00e+00 1.00e+00h 1
25 2.3924531e+00 3.84e-05 2.83e-02 -4.0 4.31e-02 -0.2 1.00e+00 3.99e-01h 1
26 2.3743716e+00 9.92e-05 1.98e-02 -4.0 8.98e-02 -0.7 1.00e+00 3.45e-01h 1
27 2.3510741e+00 1.79e-04 1.35e-02 -4.0 1.75e-01 -1.2 1.00e+00 3.04e-01h 1
28 2.3271800e+00 7.23e-03 1.54e-02 -3.3 2.88e-01 -1.6 1.00e+00 4.42e-01h 1
29 2.3008947e+00 1.59e-02 8.77e-03 -4.0 4.52e-01 -2.1 4.05e-01 4.80e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 2.3029145e+00 1.93e-02 2.89e-02 -3.3 7.26e-01 -2.6 1.00e+00 5.35e-01h 1
31 2.2981884e+00 3.76e-02 1.86e-02 -3.4 3.93e-01 - 8.77e-01 1.00e+00h 1
32 2.3165016e+00 2.01e-02 1.63e-02 -3.1 5.83e-01 -3.1 1.00e+00 9.70e-01h 1
33 2.3077911e+00 3.77e-03 4.61e-03 -3.2 3.15e-01 - 1.00e+00 1.00e+00h 1
34 2.2770538e+00 1.96e-03 2.04e-03 -4.0 1.70e-01 - 9.86e-01 1.00e+00h 1
35 2.2752277e+00 3.36e-04 2.32e-04 -4.0 8.97e-02 - 1.00e+00 1.00e+00h 1
36 2.2751934e+00 1.97e-05 1.22e-05 -4.0 1.92e-02 - 1.00e+00 1.00e+00h 1
37 2.2751890e+00 6.93e-09 3.47e-09 -4.0 3.30e-04 - 1.00e+00 1.00e+00h 1
Number of Iterations....: 37
(scaled) (unscaled)
Objective...............: 2.2751889656507767e+00 2.2751889656507767e+00
Dual infeasibility......: 3.4673572110841162e-09 3.4673572110841162e-09
Constraint violation....: 6.9310468475691778e-09 6.9310468475691778e-09
Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00
Complementarity.........: 1.0000025319088483e-04 1.0000025319088483e-04
Overall NLP error.......: 6.9310468475691778e-09 6.9310468475691778e-09
Number of objective function evaluations = 38
Number of objective gradient evaluations = 38
Number of equality constraint evaluations = 38
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 38
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 37
Total seconds in IPOPT = 10.505
EXIT: Optimal Solution Found.Visualize Solution
plot(prob.trajectory) # See NamedTrajectories.jl documentation for plotting options
Analyze Solution
x_sol = prob.trajectory.x
u_sol = prob.trajectory.u
Δt_sol = prob.trajectory.Δt
println("Solution found!")
println(" Total time: $(sum(Δt_sol)) seconds")
println(" Δt range: [$(minimum(Δt_sol)), $(maximum(Δt_sol))]")
println(" Max |u₁|: $(maximum(abs.(u_sol[1,:])))")
println(" Max |u₂|: $(maximum(abs.(u_sol[2,:])))")
println(" Final error: $(norm(x_sol[:,end] - x_goal))")Solution found!
Total time: 4.246171930683484 seconds
Δt range: [0.07285796356082225, 0.1749999999999999]
Max |u₁|: 0.9967728853909472
Max |u₂|: 0.9979498454397794
Final error: 0.0Key Insights
Free time optimization: Variable Δt allows the optimizer to adjust trajectory speed, with shorter steps where control is needed and longer steps in smooth regions.
Control bounds: With time weight 0.5, controls don't fully saturate. Increase the weight to push toward bang-bang control.
Combined objectives: The + operator makes it easy to balance multiple goals.
Exercises
1. Bang-bang control: Set time weight to 5.0 - do controls saturate the bounds?
2. Fixed time: Remove Δt from controls and compare total time.
3. Add waypoint: Require passing through [0.5, 0, 0.5] at the midpoint:
constraint = NonlinearKnotPointConstraint(
x -> x - [0.5, 0, 0.5], :x, traj;
times=[div(N,2)], equality=true
)
prob = DirectTrajOptProblem(traj, obj, integrator; constraints=[constraint])4. Different goal: Try reaching [0, 1, 0] or [0.5, 0.5, 0.5]
5. Tighter bounds: Use bounds=(u = 0.5, Δt = (0.05, 0.3)) - how does time change?
This page was generated using Literate.jl.