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...
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.6843619e+00 1.49e-01 1.03e-01 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 2.8724805e+00 1.28e-01 2.10e+00 -0.4 2.74e+00 - 2.75e-01 1.32e-01h 1
2 2.7317967e+00 9.20e-02 8.11e+00 -4.0 2.36e+00 - 2.90e-01 3.07e-01h 1
3 2.6257994e+00 8.92e-02 3.68e+02 -0.2 1.03e+00 2.0 1.00e+00 3.13e-02h 1
4 2.7876948e+00 8.02e-02 3.05e+02 0.5 1.49e+00 1.5 1.00e+00 1.21e-01h 1
5 3.5890324e+00 7.50e-02 3.33e+02 1.9 1.68e+01 1.0 3.39e-01 5.26e-02f 1
6 4.9658636e+00 1.56e-01 4.20e+02 1.8 2.92e+00 - 3.59e-01 3.14e-01f 1
7 4.9841185e+00 2.14e-01 6.85e+02 1.4 1.43e+00 1.5 4.76e-01 8.01e-01f 1
8 4.6730116e+00 1.01e-01 1.86e+02 1.1 2.95e-01 1.9 9.91e-01 9.12e-01f 1
9 4.6445332e+00 2.29e-02 3.52e+01 0.1 3.27e-01 - 9.07e-01 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 4.6185611e+00 9.16e-05 3.10e-01 -1.3 3.07e-02 - 9.96e-01 1.00e+00h 1
11 4.2081040e+00 7.91e-02 4.87e-01 -1.9 1.10e+00 - 6.98e-01 1.00e+00f 1
12 4.1727909e+00 5.66e-03 6.10e+00 -2.0 2.08e-01 1.4 9.68e-01 1.00e+00h 1
13 4.1703396e+00 4.68e-05 8.21e-01 -3.4 1.16e-02 1.8 1.00e+00 1.00e+00h 1
14 4.1667917e+00 2.29e-06 9.79e-02 -4.0 4.19e-03 1.4 1.00e+00 1.00e+00h 1
15 4.1564348e+00 1.92e-05 9.58e-02 -4.0 1.23e-02 0.9 1.00e+00 1.00e+00f 1
16 4.1270626e+00 1.46e-04 9.00e-02 -4.0 3.46e-02 0.4 1.00e+00 1.00e+00f 1
17 4.0512502e+00 8.40e-04 7.67e-02 -4.0 8.77e-02 -0.1 1.00e+00 1.00e+00f 1
18 4.0288427e+00 7.74e-04 5.92e-02 -4.0 1.53e-01 -0.5 1.00e+00 1.41e-01h 1
19 3.9716791e+00 4.79e-04 5.49e-02 -4.0 7.12e-02 -0.1 1.00e+00 1.00e+00h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
20 3.9466676e+00 4.42e-04 3.99e-02 -4.0 1.30e-01 -0.6 1.00e+00 1.83e-01h 1
21 3.8969972e+00 3.26e-04 3.82e-02 -4.0 5.58e-02 -0.2 1.00e+00 1.00e+00h 1
22 3.8664210e+00 3.51e-04 3.00e-02 -4.0 1.11e-01 -0.6 1.00e+00 2.64e-01h 1
23 3.8434332e+00 2.11e-04 2.71e-02 -4.0 4.45e-02 -0.2 1.00e+00 5.92e-01h 1
24 3.8092410e+00 2.81e-04 2.49e-02 -4.0 1.18e-01 -0.7 1.00e+00 3.52e-01h 1
25 3.7501277e+00 7.28e-04 2.19e-02 -4.0 2.46e-01 -1.2 1.00e+00 2.75e-01f 1
26 3.7037034e+00 7.17e-04 1.95e-02 -4.0 1.05e-01 -0.7 1.00e+00 6.19e-01h 1
27 3.6546159e+00 9.70e-04 1.79e-02 -4.0 2.40e-01 -1.2 1.00e+00 3.01e-01h 1
28 3.6266991e+00 6.88e-04 1.66e-02 -4.0 9.67e-02 -0.8 1.00e+00 5.04e-01h 1
29 3.5841452e+00 1.32e-03 1.49e-02 -4.0 2.36e-01 -1.3 9.87e-01 3.50e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
30 3.5438550e+00 1.63e-03 1.36e-02 -4.0 9.55e-02 -0.8 1.00e+00 9.74e-01h 1
31 3.4992719e+00 8.61e-03 9.59e-03 -3.2 2.62e-01 -1.3 1.00e+00 7.03e-01f 1
32 3.4526487e+00 1.36e-02 1.46e-02 -3.4 5.46e-01 -1.8 9.98e-01 4.78e-01h 1
33 3.4287835e+00 1.65e-02 1.96e-02 -3.0 1.03e+00 -2.3 1.00e+00 3.13e-01h 1
34 3.5269720e+00 2.55e-01 1.06e-01 -2.2 5.39e+00 -2.8 1.70e-01 1.64e-01f 1
35 3.4899499e+00 1.64e-01 7.53e-02 -2.9 7.91e-01 -2.3 4.91e-01 3.50e-01h 1
36 3.4584252e+00 1.14e-01 4.64e-02 -2.9 1.31e+00 -2.8 6.05e-01 3.48e-01h 1
37 3.4040816e+00 2.06e-02 3.15e-02 -2.9 2.19e-01 -1.5 7.97e-01 1.00e+00h 1
38 3.3349761e+00 1.30e-02 1.39e-02 -3.6 1.50e-01 -1.1 9.71e-01 1.00e+00h 1
39 3.2966732e+00 4.76e-02 6.44e-02 -2.5 2.12e+00 -1.5 3.75e-01 2.09e-01f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
40 3.2365461e+00 4.36e-02 2.88e-01 -2.6 7.26e-01 -1.1 9.56e-01 2.09e-01h 1
41 3.0953650e+00 7.49e-03 8.94e-02 -3.0 1.65e-01 -0.7 9.96e-01 1.00e+00h 1
42 3.5037786e+00 1.28e-01 4.61e-01 -1.0 7.15e+00 -1.2 2.56e-01 6.36e-02f 1
43 3.4550934e+00 1.27e-01 7.54e-01 -1.6 1.25e+01 - 8.52e-02 1.15e-02h 1
44 3.0748589e+00 1.22e-01 7.51e-01 -1.6 2.91e+01 - 1.93e-02 3.94e-02f 1
45 2.9120999e+00 5.41e-02 3.72e-01 -3.5 9.72e-01 - 4.37e-01 5.55e-01h 1
46 2.6395801e+00 3.11e-02 2.96e-01 -2.2 1.44e+00 - 5.02e-01 7.14e-01h 1
47 2.5903697e+00 2.51e-02 1.94e-01 -2.9 2.70e+00 - 4.04e-01 1.98e-01h 1
48 2.4763208e+00 4.89e-03 2.13e-02 -2.7 3.00e-01 -1.6 9.95e-01 1.00e+00h 1
49 2.3543265e+00 7.23e-03 1.55e-02 -3.3 4.50e-01 -2.1 9.30e-01 8.81e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
50 2.3391501e+00 3.07e-02 1.75e-02 -3.2 1.37e+00 -2.6 4.77e-01 3.92e-01h 1
Number of Iterations....: 50
(scaled) (unscaled)
Objective...............: 2.3391500539864500e+00 2.3391500539864500e+00
Dual infeasibility......: 1.7535423119024989e-02 1.7535423119024989e-02
Constraint violation....: 3.0691433826820713e-02 3.0691433826820713e-02
Variable bound violation: 0.0000000000000000e+00 0.0000000000000000e+00
Complementarity.........: 1.3176294361339721e-03 1.3176294361339721e-03
Overall NLP error.......: 3.0691433826820713e-02 3.0691433826820713e-02
Number of objective function evaluations = 51
Number of objective gradient evaluations = 51
Number of equality constraint evaluations = 51
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.475
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.431125212321699 seconds
Δt range: [0.062044280280519185, 0.1750000010138639]
Max |u₁|: 0.9766185264576638
Max |u₂|: 0.9962088612658118
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.