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.219934 -0.062434 0.238693 … -0.106296 0.0393956 -0.137623
-0.0134449 -0.00111192 -0.0138572 -0.0617812 0.124917 -0.0934463Step 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
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.4101998e-02 1.52e-01 3.60e+252 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 1.8829604e-02 1.52e-01 1.80e+00 -2.0 1.72e+252 - 5.02e-253 5.02e-253s238
2 1.4303838e-02 1.43e-01 1.18e+01 -0.2 5.49e+00 - 5.58e-01 6.09e-02h 1
Number of Iterations....: 3
Number of objective function evaluations = 242
Number of objective gradient evaluations = 4
Number of equality constraint evaluations = 242
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 3
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 3
Total seconds in IPOPT = 6.857
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.19855100658160055
0.3686240613534652
0.5472327067434213
0.7161134095374455
0.8421270070248534
0.9534015877428199
1.076430695412426
1.158670959285053
1.3015843959150082
⋮
5.10738409344893
5.179971336049407
5.248842132434246
5.31651199591451
5.379079588275942
5.4513742667047165
5.518279396860085
5.519437604423144
5.709104336017177Goal 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.2919683613188038
Mean magnitude: 0.07774007997551717
Total norm: 0.8194367316761607
✓ Bounds satisfied
u2:
Max magnitude: 0.6974758126142808
Mean magnitude: 0.1850306473824016
Total norm: 1.821535576829205
✓ 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.0068323669832198
Max |x₂|: 0.33304653429234343
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.30 | 0.7112 | -0.3013 | 0.2021
2.06 | 0.4323 | -0.0143 | 0.4000
2.83 | 0.5068 | 0.1299 | 0.5175
4.08 | 0.4276 | -0.0442 | 0.6605
4.89 | 0.1796 | -0.0768 | 0.8138
5.52 | 0.0000 | 0.0000 | 1.0000
Control trajectory (selected time points):
Time | u₁ | u₂
------------------------------
0.00 | 0.2802 | -0.2024
1.30 | -0.0420 | -0.3264
2.06 | 0.0467 | -0.0891
2.83 | -0.0624 | 0.1039
4.08 | -0.0473 | -0.1771
4.89 | -0.0223 | 0.0071
5.52 | -0.1307 | -0.0887Step 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.11769007451980365Comparison: 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
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 = 0.003
EXIT: Invalid number in NLP function or derivative detected.
Comparison:
Original (R=1.0):
||u||: 1.9973653683977335
High weight (R=10.0):
||u||: 1.0768999203119194
Control reduction: 46.08397955874254%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.