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)
probDirectTrajOptProblem
Trajectory
Timesteps: 50
Duration: 7.35
Knot dim: 6
Variables: x (3), u (2), Δt (1)
Controls: u, Δt
Objective (2 terms)
1.0 * QuadraticRegularizer on :u (R = [1.0, 1.0], all)
0.5 * MinimumTimeObjective (D = 1.0)
Dynamics (1 integrators)
BilinearIntegrator: :x = exp(Δt G(:u)) :x (dim = 3)
Constraints (4 total: 2 equality, 2 bounds)
EqualityConstraint: "initial value of x"
EqualityConstraint: "final value of x"
BoundsConstraint: "bounds on u"
BoundsConstraint: "bounds on Δt"solve!(prob; max_iter = 50) initializing optimizer...
building evaluator: 1 integrators, 0 nonlinear constraints
dynamics constraints: 147, nonlinear constraints: 0
integrator 1 jacobian structure: 0.364s
jacobian structure: 1764 nonzeros (0.386s)
integrator 1 hessian structure: 0.02s
computing objective hessian structure (CompositeObjective)...
sub-objective 1 (QuadraticRegularizer): 0.37s
sub-objective 2 (MinimumTimeObjective): 0.007s
objective hessian structure: 0.436s
hessian structure: 2814 nonzeros (0.456s)
linear index maps built (0.004s)
evaluator ready (total: 1.661s)
evaluator created (2.227s)
NL constraint bounds extracted (0.027s)
NLP block data built (0.0s)
Ipopt optimizer configured (0.01s)
variables set (0.143s)
applying constraint: initial value of x
applying constraint: final value of x
applying constraint: bounds on u
applying constraint: bounds on Δt
linear constraints added: 4 (0.479s)
optimizer initialization complete (total: 2.886s)
******************************************************************************
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.6877873e+00 1.50e-01 1.04e-01 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 3.0115141e+00 1.33e-01 6.97e-01 -4.0 3.09e+00 - 1.47e-01 1.16e-01h 1
2 2.9609244e+00 1.06e-01 3.95e+00 -0.4 1.85e+00 - 4.35e-01 2.06e-01f 1
3 2.6957477e+00 8.48e-02 1.34e+01 -0.3 1.83e+00 0.0 6.50e-01 1.97e-01h 1
4 2.7552484e+00 7.21e-02 2.32e+01 0.0 3.73e+00 - 5.78e-01 1.65e-01h 1
5 2.6277848e+00 6.71e-02 4.46e+01 -4.0 4.63e+00 - 1.45e-01 2.26e-01h 1
6 2.8570430e+00 5.90e-02 8.60e+01 0.7 3.29e+00 1.3 4.67e-01 1.32e-01f 1
7 3.2284918e+00 5.18e-02 1.01e+02 1.0 1.00e+00 1.8 4.03e-01 2.86e-01f 1
8 3.3450390e+00 2.78e-02 3.11e+02 0.8 2.68e-01 3.1 8.54e-01 6.80e-01h 1
9 3.2809166e+00 1.98e-03 2.27e+02 0.2 6.56e-02 3.5 9.91e-01 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 3.2785982e+00 2.26e-06 3.34e+00 -1.0 3.64e-03 3.0 1.00e+00 1.00e+00h 1
11 3.2776145e+00 1.47e-07 1.39e-01 -2.7 3.90e-04 2.6 1.00e+00 1.00e+00h 1
12 3.2733908e+00 1.70e-06 3.09e-01 -4.0 2.58e-03 2.1 1.00e+00 1.00e+00f 1
13 3.2635405e+00 8.75e-06 2.64e-01 -4.0 6.64e-03 1.6 1.00e+00 8.23e-01f 1
14 3.2396016e+00 7.73e-05 1.10e-01 -4.0 8.30e-03 1.1 1.00e+00 1.00e+00f 1
15 3.2218786e+00 9.92e-05 9.87e-02 -4.0 2.13e-02 0.6 1.00e+00 2.95e-01f 1
16 3.1761370e+00 3.68e-04 9.30e-02 -4.0 5.63e-02 0.2 1.00e+00 3.31e-01f 1
17 3.1432496e+00 3.23e-04 9.15e-02 -4.0 2.30e-02 0.6 1.00e+00 6.91e-01h 1
18 3.1058113e+00 4.76e-04 8.58e-02 -4.0 5.95e-02 0.1 1.00e+00 3.57e-01f 1
19 3.0940742e+00 1.05e-03 9.98e-02 -3.1 5.78e-01 -0.4 1.00e+00 6.08e-02f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 3.0329102e+00 1.01e-03 6.86e-02 -3.5 5.90e-02 0.1 1.00e+00 7.12e-01h 1
21 2.9470952e+00 1.90e-03 5.34e-02 -4.0 1.24e-01 -0.4 1.00e+00 4.93e-01f 1
22 2.8917173e+00 8.90e-04 4.66e-02 -4.0 4.48e-02 0.0 1.00e+00 1.00e+00h 1
23 2.8233672e+00 1.26e-03 4.14e-02 -4.0 1.15e-01 -0.5 1.00e+00 5.55e-01h 1
24 2.7765235e+00 1.27e-03 1.29e-01 -4.0 2.94e-01 -0.9 1.00e+00 2.13e-01h 1
25 2.7260260e+00 9.38e-04 3.57e-02 -4.0 1.11e-01 -0.5 1.00e+00 5.83e-01h 1
26 2.6498084e+00 2.07e-03 4.98e-02 -4.0 2.49e-01 -1.0 1.00e+00 4.43e-01h 1
27 2.5962384e+00 8.45e-04 2.24e-02 -4.0 8.25e-02 -0.6 1.00e+00 1.00e+00h 1
28 2.5517240e+00 1.21e-03 3.47e-02 -4.0 1.93e-01 -1.0 1.00e+00 4.00e-01h 1
29 2.4722471e+00 8.29e-03 8.50e-02 -3.7 8.69e-01 -1.5 1.00e+00 4.24e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 2.4393137e+00 2.38e-03 1.49e-02 -3.4 1.81e-01 -1.1 1.00e+00 7.66e-01h 1
31 2.4190150e+00 2.15e-03 8.99e-02 -3.1 5.06e-01 -1.6 9.02e-01 3.43e-01h 1
32 2.3760924e+00 2.69e-03 1.45e-02 -4.0 2.14e-01 -1.1 1.00e+00 1.00e+00h 1
33 2.3676627e+00 2.55e-03 1.48e-02 -3.3 4.48e-01 -1.6 1.00e+00 2.55e-01h 1
34 2.3597778e+00 4.68e-03 4.71e-02 -3.0 1.00e+00 -2.1 1.00e+00 2.96e-01h 1
35 2.3460750e+00 4.37e-03 4.63e-02 -3.3 1.06e+00 -2.6 5.23e-01 2.30e-01h 1
36 2.3086859e+00 5.64e-03 1.31e-02 -3.9 3.92e-01 -2.1 1.00e+00 7.73e-01h 1
37 2.3019930e+00 1.05e-02 9.30e-03 -3.5 7.61e-01 -2.6 7.86e-01 4.58e-01h 1
38 2.2964308e+00 1.25e-02 1.51e-02 -4.0 1.69e+00 -3.1 2.13e-01 3.10e-01h 1
39 2.2825745e+00 3.00e-04 1.71e-03 -4.0 1.00e-01 -1.8 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 2.2798239e+00 3.47e-03 1.80e-03 -4.0 1.52e-01 -2.2 1.00e+00 1.00e+00h 1
41 2.2796387e+00 5.06e-04 7.64e-04 -4.0 5.02e-02 -1.8 1.00e+00 1.00e+00h 1
42 2.2782302e+00 1.21e-02 9.46e-03 -4.0 3.62e-01 -2.3 1.00e+00 7.87e-01h 1
43 2.2945610e+00 2.08e-02 1.35e-02 -3.5 7.60e-01 - 6.86e-01 8.59e-01f 1
44 2.2910259e+00 1.62e-02 3.19e-02 -3.7 5.20e+00 - 7.88e-02 3.58e-01H 1
45 2.2830079e+00 9.56e-03 6.12e-03 -3.7 3.66e-01 - 1.00e+00 1.00e+00f 1
46 2.2797193e+00 3.35e-02 2.76e-03 -3.7 5.21e-01 - 1.00e+00 1.00e+00H 1
47 2.2805662e+00 2.64e-02 1.26e-03 -3.7 1.76e+00 - 2.39e-01 1.28e-01h 2
48 2.2829902e+00 4.63e-04 4.97e-04 -3.7 3.83e-02 -1.9 1.00e+00 1.00e+00h 1
49 2.2761754e+00 9.77e-04 2.74e-04 -4.0 1.58e-01 - 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
50 2.2758789e+00 5.24e-04 4.14e-04 -4.0 6.10e-02 -2.4 1.00e+00 1.00e+00h 1
Number of Iterations....: 50
(scaled) (unscaled)
Objective...............: 2.2758788808777641e+00 2.2758788808777641e+00
Dual infeasibility......: 4.1363121918879127e-04 4.1363121918879127e-04
Constraint violation....: 5.2360898914183274e-04 5.2360898914183274e-04
Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00
Complementarity.........: 1.0737181156625764e-04 1.0737181156625764e-04
Overall NLP error.......: 5.2360898914183274e-04 5.2360898914183274e-04
Number of objective function evaluations = 54
Number of objective gradient evaluations = 51
Number of equality constraint evaluations = 54
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 51
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 50
Total seconds in IPOPT = 12.165
EXIT: Maximum Number of Iterations Exceeded.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.246402179747298 seconds
Δt range: [0.0709028222477025, 0.175]
Max |u₁|: 0.9965828144358863
Max |u₂|: 0.9978437284000461
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.