Tutorial: Bilinear Control with Multiple Drives
This tutorial demonstrates a more complex bilinear control problem with:
- Multiple control inputs
- Control bounds
- A 3D state space
Problem Description
Control a 3D system with 2 independent control inputs.
Dynamics:
\[\dot{x} = (G_0 + u_1 G_1 + u_2 G_2) x\]
Goal: Navigate from [1, 0, 0] to [0, 0, 1] with bounded controls
using DirectTrajOpt
using NamedTrajectories
using LinearAlgebra
using Statistics
using PrintfStep 1: Define the System
3D System with Two Drives
Drift term - natural evolution
G_drift = [
0.0 1.0 0.0;
-1.0 0.0 0.0;
0.0 0.0 -0.1
]3×3 Matrix{Float64}:
0.0 1.0 0.0
-1.0 0.0 0.0
0.0 0.0 -0.1Drive terms - control influences
G_drive_1 = [
1.0 0.0 0.0;
0.0 0.0 0.0;
0.0 0.0 0.0
] # Controls first state
G_drive_2 = [
0.0 0.0 0.0;
0.0 0.0 1.0;
0.0 1.0 0.0
] # Controls coupling between states 2 and 3
G_drives = [G_drive_1, G_drive_2]2-element Vector{Matrix{Float64}}:
[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]Generator function
G = u -> G_drift + sum(u .* G_drives)
println("System dynamics:")
println(" State dimension: 3")
println(" Number of controls: 2")
println(" G_drift creates oscillation in x₁-x₂ plane with decay in x₃")
println(" u₁ drives x₁ component")
println(" u₂ couples x₂ and x₃")System dynamics:
State dimension: 3
Number of controls: 2
G_drift creates oscillation in x₁-x₂ plane with decay in x₃
u₁ drives x₁ component
u₂ couples x₂ and x₃Step 2: Set Up the Problem
Time Discretization
N = 60
Δt = 0.15
total_time = N * Δt
println("\nTime discretization:")
println(" Time steps: $N")
println(" Step size: $Δt")
println(" Total time: $total_time seconds")
Time discretization:
Time steps: 60
Step size: 0.15
Total time: 9.0 secondsBoundary Conditions
x_init = [1.0, 0.0, 0.0]
x_goal = [0.0, 0.0, 1.0]
println("\nBoundary conditions:")
println(" Initial state: $x_init")
println(" Goal state: $x_goal")
Boundary conditions:
Initial state: [1.0, 0.0, 0.0]
Goal state: [0.0, 0.0, 1.0]Initial Guess
Linear interpolation for states
x_guess = hcat([x_init + (x_goal - x_init) * (t/(N-1)) for t = 0:(N-1)]...)3×60 Matrix{Float64}:
1.0 0.983051 0.966102 0.949153 … 0.0338983 0.0169492 0.0
0.0 0.0 0.0 0.0 0.0 0.0 0.0
0.0 0.0169492 0.0338983 0.0508475 0.966102 0.983051 1.0Small random controls
u_guess = 0.1 * randn(2, N)2×60 Matrix{Float64}:
-0.0717174 0.0240672 -0.10848 … -0.0936848 -0.0007471 0.00922714
0.284452 -0.00618757 0.0363562 -0.216696 0.104984 -0.0651019Step 3: Create Trajectory with Bounds
Control bounds: -1 ≤ u ≤ 1 for both controls
traj = NamedTrajectory(
(x = x_guess, u = u_guess, Δt = fill(Δt, N));
timestep = :Δt,
controls = :u,
initial = (x = x_init,),
final = (x = x_goal,),
bounds = (u = 1.0,), # -1 ≤ u ≤ 1
)
println("\nTrajectory created:")
println(" Control bounds: ", traj.bounds.u)
Trajectory created:
Control bounds: ([-1.0, -1.0], [1.0, 1.0])Step 4: Define Dynamics and Objective
Dynamics integrator
integrator = BilinearIntegrator(G, :x, :u, traj)BilinearIntegrator: :x = exp(Δt G(:u)) :x (dim = 3)Objective: minimize control effort
R_u = 1.0 # control weight
obj = QuadraticRegularizer(:u, traj, R_u)
println("\nObjective: minimize ∫ ||u||² dt")
println(" Control weight: $R_u")
Objective: minimize ∫ ||u||² dt
Control weight: 1.0Step 5: Solve the Problem
prob = DirectTrajOptProblem(traj, obj, integrator)
probDirectTrajOptProblem
Trajectory
Timesteps: 60
Duration: 8.85
Knot dim: 6
Variables: x (3), u (2), Δt (1)
Controls: u, Δt
Objective: QuadraticRegularizer on :u (R = [1.0, 1.0], all)
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"println("Solving optimization problem...")
println("="^50)
solve!(prob; max_iter = 150, verbose = false)
println("="^50)
println("Optimization complete!")
println("="^50)Solving optimization problem...
==================================================
This is Ipopt version 3.14.19, running with linear solver MUMPS 5.8.2.
Number of nonzeros in equality constraint Jacobian...: 2106
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 3318
Number of Iterations....: 0
Number of objective function evaluations = 0
Number of objective gradient evaluations = 1
Number of equality constraint evaluations = 0
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 1
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 0
Total seconds in IPOPT = 2.326
EXIT: Invalid number in NLP function or derivative detected.
==================================================
Optimization complete!
==================================================Step 6: Analyze the Solution
x_sol = prob.trajectory.x
u_sol = prob.trajectory.u
times = cumsum([0.0; prob.trajectory.Δt[:]])61-element Vector{Float64}:
0.0
0.15
0.3
0.44999999999999996
0.6
0.75
0.9
1.05
1.2
1.3499999999999999
⋮
7.800000000000008
7.950000000000008
8.100000000000009
8.250000000000009
8.40000000000001
8.55000000000001
8.70000000000001
8.85000000000001
9.00000000000001Goal Reaching
println("\nGoal reaching:")
println(" Initial state: ", x_sol[:, 1])
println(" Final state: ", x_sol[:, end])
println(" Goal state: ", x_goal)
println(" Final error: ", norm(x_sol[:, end] - x_goal))
Goal reaching:
Initial state: [1.0, 0.0, 0.0]
Final state: [0.0, 0.0, 1.0]
Goal state: [0.0, 0.0, 1.0]
Final error: 0.0Control Statistics
println("\nControl statistics:")
for i = 1:2
u_i = u_sol[i, :]
println(" u$i:")
println(" Max magnitude: ", maximum(abs.(u_i)))
println(" Mean magnitude: ", mean(abs.(u_i)))
println(" Total norm: ", norm(u_i))
# Check bound satisfaction
if all(-1.0 .<= u_i .<= 1.0)
println(" ✓ Bounds satisfied")
else
println(" ✗ Bounds violated!")
end
end
Control statistics:
u1:
Max magnitude: 0.25564943283879843
Mean magnitude: 0.07740030636097102
Total norm: 0.727240595450778
✓ Bounds satisfied
u2:
Max magnitude: 0.32363014928736933
Mean magnitude: 0.08362346266893204
Total norm: 0.8375192693780075
✓ Bounds satisfiedState Trajectory Analysis
println("\nState trajectory:")
println(" Max |x₁|: ", maximum(abs.(x_sol[1, :])))
println(" Max |x₂|: ", maximum(abs.(x_sol[2, :])))
println(" Max |x₃|: ", maximum(abs.(x_sol[3, :])))
State trajectory:
Max |x₁|: 1.0
Max |x₂|: 0.0
Max |x₃|: 1.0Step 7: Detailed Results
println("\n" * "="^50)
println("SOLUTION DETAILS")
println("="^50)
println("\nState trajectory (selected time points):")
println("Time | x₁ | x₂ | x₃")
println("-"^40)
for k in [1, 10, 20, 30, 40, 50, N]
t = times[k]
println(
@sprintf("%.2f | %7.4f | %7.4f | %7.4f", t, x_sol[1, k], x_sol[2, k], x_sol[3, k])
)
end
println("\nControl trajectory (selected time points):")
println("Time | u₁ | u₂")
println("-"^30)
for k in [1, 10, 20, 30, 40, 50, N]
t = times[k]
println(@sprintf("%.2f | %7.4f | %7.4f", t, u_sol[1, k], u_sol[2, k]))
end
==================================================
SOLUTION DETAILS
==================================================
State trajectory (selected time points):
Time | x₁ | x₂ | x₃
----------------------------------------
0.00 | 1.0000 | 0.0000 | 0.0000
1.35 | 0.8475 | 0.0000 | 0.1525
2.85 | 0.6780 | 0.0000 | 0.3220
4.35 | 0.5085 | 0.0000 | 0.4915
5.85 | 0.3390 | 0.0000 | 0.6610
7.35 | 0.1695 | 0.0000 | 0.8305
8.85 | 0.0000 | 0.0000 | 1.0000
Control trajectory (selected time points):
Time | u₁ | u₂
------------------------------
0.00 | -0.0717 | 0.2845
1.35 | -0.2107 | -0.1982
2.85 | 0.0294 | -0.0671
4.35 | 0.0253 | 0.1816
5.85 | -0.0847 | 0.0447
7.35 | -0.0235 | 0.0302
8.85 | 0.0092 | -0.0651Step 8: Verify Dynamics Satisfaction
println("\nDynamics verification:")
max_error = 0.0
for k = 1:(N-1)
global max_error
x_k = x_sol[:, k]
u_k = u_sol[:, k]
Δt_k = prob.trajectory.Δt[k]
# Predicted next state
x_k1_pred = exp(Δt_k * G(u_k)) * x_k
x_k1_actual = x_sol[:, k+1]
err = norm(x_k1_pred - x_k1_actual)
max_error = max(max_error, err)
end
println(" Maximum dynamics error: $max_error")
Dynamics verification:
Maximum dynamics error: 0.150117049740184Comparison: Different Control Weights
println("\n" * "="^50)
println("EXPLORING DIFFERENT CONTROL WEIGHTS")
println("="^50)
==================================================
EXPLORING DIFFERENT CONTROL WEIGHTS
==================================================Try with higher control penalty
traj_high = NamedTrajectory(
(x = x_guess, u = 0.1*randn(2, N), Δt = fill(Δt, N));
timestep = :Δt,
controls = :u,
initial = (x = x_init,),
final = (x = x_goal,),
bounds = (u = 1.0,),
)
obj_high = QuadraticRegularizer(:u, traj_high, 10.0) # 10x larger weight
integrator_high = BilinearIntegrator(G, :x, :u, traj_high)
prob_high = DirectTrajOptProblem(traj_high, obj_high, integrator_high)
println("\nSolving with high control weight (R=10.0)...")
solve!(prob_high; max_iter = 150, verbose = false)
u_sol_high = prob_high.trajectory.u
println("\nComparison:")
println(" Original (R=1.0):")
println(" ||u||: ", norm(u_sol))
println(" High weight (R=10.0):")
println(" ||u||: ", norm(u_sol_high))
println(" Control reduction: ", (1 - norm(u_sol_high)/norm(u_sol)) * 100, "%")┌ Warning: Trajectory has timestep variable :Δt but no bounds on it.
│ Adding default lower bound of 0 to prevent negative timesteps.
│
│ Recommended: Add explicit bounds when creating the trajectory:
│ NamedTrajectory(...; Δt_bounds=(min, max))
│ Example:
│ NamedTrajectory(qtraj, N; Δt_bounds=(1e-3, 0.5))
│
│ Or use timesteps_all_equal=true in problem options to fix timesteps.
└ @ DirectTrajOpt.Problems ~/work/DirectTrajOpt.jl/DirectTrajOpt.jl/src/problems.jl:66
Solving with high control weight (R=10.0)...
This is Ipopt version 3.14.19, running with linear solver MUMPS 5.8.2.
Number of nonzeros in equality constraint Jacobian...: 2106
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 3318
Total number of variables............................: 354
variables with only lower bounds: 60
variables with lower and upper bounds: 120
variables with only upper bounds: 0
Total number of equality constraints.................: 177
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 1.5963869e-01 1.49e-01 9.96e-01 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 6.7092673e-02 1.15e-01 1.43e+00 -4.0 6.46e-01 0.0 7.44e-01 2.30e-01h 1
2 1.7642867e-01 1.01e-01 1.06e+270 -0.8 2.14e+00 0.4 2.86e-01 1.19e-01h 1
3 1.7547309e-01 1.01e-01 1.80e+01 -2.5 1.84e+268 1.8 1.08e-269 1.08e-269s 2
4 3.7107681e-01 9.31e-02 1.02e+02 -0.3 2.60e+00 1.3 1.00e+00 7.75e-02h 1
5 5.6646506e-01 8.71e-02 4.16e+02 0.2 2.28e+00 1.7 1.00e+00 6.42e-02h 1
6 8.6533237e-01 8.65e-02 1.06e+270 2.2 3.75e+01 1.2 2.38e-01 7.96e-03f 1
7 8.6764874e-01 8.65e-02 1.42e+03 1.4 1.83e+266 - 6.55e-268 6.55e-268s 2
8 2.0557334e+00 1.44e-01 2.93e+03 1.4 3.41e+00 2.6 3.05e-01 0.00e+00S 2
9 3.7900992e+00 1.48e-01 2.47e+03 -4.0 1.97e+00 - 2.36e-01 1.32e-01h 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 6.0811914e+00 1.30e-01 2.44e+03 -4.0 2.24e+00 - 2.02e-01 1.19e-01h 1
11 7.8767870e+00 1.20e-01 1.82e+04 1.6 3.49e+00 2.1 1.00e+00 7.52e-02h 1
12 7.9695226e+00 1.13e-01 1.74e+04 -4.0 5.69e+00 - 8.15e-02 6.33e-02h 1
13 5.7704229e+00 1.06e-01 1.60e+04 -4.0 6.55e+00 - 1.04e-01 6.37e-02f 1
14 5.8147619e+00 1.06e-01 1.03e+04 -0.1 5.33e+00 - 2.84e-01 3.31e-02h 2
15 7.3300188e+00 1.15e-01 9.06e+03 -4.0 3.63e+00 - 8.27e-03 5.74e-02h 1
16 7.7400610e+00 1.14e-01 8.14e+241 -4.0 7.45e+00 - 1.39e-03 1.48e-02h 1
17 8.4482147e+00 1.14e-01 8.16e+03 -0.4 1.54e+240 - 6.24e-241 6.24e-241s278
Number of Iterations....: 18
Number of objective function evaluations = 303
Number of objective gradient evaluations = 19
Number of equality constraint evaluations = 303
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 18
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 18
Total seconds in IPOPT = 4.591
EXIT: Invalid number in NLP function or derivative detected.
Comparison:
Original (R=1.0):
||u||: 1.1091967410027284
High weight (R=10.0):
||u||: 5.114984700224612
Control reduction: -361.1431418019313%Key Observations
- Multiple controls allow independent actuation of different system modes
- Bounds are strictly enforced — check that max|u| ≤ 1
- Control weight affects aggressiveness: lower weight produces larger controls that may saturate bounds, higher weight produces gentler controls
- Initial guess matters for bounded problems — random works here, but more complex problems may need better initialization
- BilinearIntegrator handles multi-input systems naturally — just provide all drive matrices in a vector
Exercises
Try these modifications:
Exercise 1: Asymmetric Bounds
Set different bounds for each control:
bounds=(u = ([-1.0, -0.5], [1.0, 2.0]),)Exercise 2: Initial Control Constraints
Start and end with zero control:
initial=(x = x_init, u = [0.0, 0.0]),
final=(x = x_goal, u = [0.0, 0.0])Exercise 3: Intermediate Waypoint
Add a waypoint constraint at t=30:
waypoint = [0.5, 0.5, 0.5]
constraint = NonlinearKnotPointConstraint(
x -> x - waypoint, :x, traj;
times=[30], equality=true
)Exercise 4: Different Goal
Try reaching [0, 1, 0] instead of [0, 0, 1]
Exercise 5: Add Minimum Time
Make Δt variable and add MinimumTimeObjective:
obj = QuadraticRegularizer(:u, traj, 1.0) +
MinimumTimeObjective(traj, 0.5)
bounds=(u = 1.0, Δt = (0.05, 0.3))Next Steps
- Minimum Time Tutorial: Optimize trajectory duration
- Smooth Controls Tutorial: Add derivative penalties for implementability
This page was generated using Literate.jl.