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.142037 0.0389806 -0.0350806 … -0.110565 0.0874951 -0.0829849
-0.0296606 0.028475 0.0626685 -0.0357051 -0.0167656 0.063805Step 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.384
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.2309571435217794
Mean magnitude: 0.0732511928723353
Total norm: 0.6919260352810085
✓ Bounds satisfied
u2:
Max magnitude: 0.24463311587062533
Mean magnitude: 0.07888284367250968
Total norm: 0.7665761982321836
✓ 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.1420 | -0.0297
1.35 | 0.0880 | -0.0655
2.85 | -0.1148 | -0.0442
4.35 | -0.0147 | -0.0897
5.85 | 0.0343 | -0.0655
7.35 | -0.1002 | 0.1140
8.85 | -0.0830 | 0.0638Step 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.14956366603269167Comparison: 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.6801963e-01 1.50e-01 1.00e+00 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 5.2712896e-02 1.13e-01 1.32e+00 -4.0 6.03e-01 0.0 6.94e-01 2.46e-01h 1
2 1.0179107e-01 1.07e-01 4.52e+00 -4.0 2.82e+00 0.4 1.32e-01 5.61e-02h 1
Number of Iterations....: 3
Number of objective function evaluations = 4
Number of objective gradient evaluations = 4
Number of equality constraint evaluations = 4
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 = 4.383
EXIT: Invalid number in NLP function or derivative detected.
Comparison:
Original (R=1.0):
||u||: 1.0326667933054705
High weight (R=10.0):
||u||: 1.8253712345526019
Control reduction: -76.76284803443308%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.