Tutorial: Linear System Control
In this tutorial, we'll solve a simple 2D linear control problem from start to finish.
Problem Description
We want to control a 2D oscillator from rest at the origin to a target position, minimizing control effort.
System dynamics:
\[\dot{x} = (G_0 + u_1 G_1) x\]
Goal: Drive from x(0) = [0, 0] to x(N) = [1, 0]
Objective: Minimize control effort ∫ ||u||² dt
using DirectTrajOpt
using NamedTrajectories
using LinearAlgebra
using Statistics
using PrintfStep 1: Define the System Dynamics
The drift matrix (natural dynamics):
G_drift = [
-0.1 1.0;
-1.0 -0.1
]2×2 Matrix{Float64}:
-0.1 1.0
-1.0 -0.1The drive matrix (control influence):
G_drives = [[
0.0 1.0;
1.0 0.0
]]1-element Vector{Matrix{Float64}}:
[0.0 1.0; 1.0 0.0]Generator function:
G = u -> G_drift + sum(u .* G_drives)#2 (generic function with 1 method)Let's understand what this system does:
- G_drift creates damped oscillations
- G_drives couples the two states symmetrically
- Control u affects how the states influence each other
Step 2: Create the Trajectory
Time parameters
N = 50# number of time steps
Δt = 0.1 # time step size
total_time = N * Δt # 5 seconds
println("Total time: $total_time seconds")Total time: 5.0 secondsInitial and goal states
x_init = [0.0, 0.0]
x_goal = [1.0, 0.0]2-element Vector{Float64}:
1.0
0.0Create initial guess with linear interpolation
x_guess = hcat([x_init + (x_goal - x_init) * (t/(N-1)) for t = 0:(N-1)]...)
u_guess = zeros(1, N)1×50 Matrix{Float64}:
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.0 0.0Create the trajectory
traj = NamedTrajectory(
(x = x_guess, u = u_guess, Δt = fill(Δt, N));
timestep = :Δt,
controls = :u,
initial = (x = x_init,),
final = (x = x_goal,),
)
println("Trajectory dimensions:")
println(" States: ", traj.dims.x)
println(" Controls: ", traj.dims.u)
println(" Time steps: ", traj.N)Trajectory dimensions:
States: 2
Controls: 1
Time steps: 50Step 3: Define the Dynamics Constraint
Use BilinearIntegrator for our control-linear system:
integrator = BilinearIntegrator(G, :x, :u, traj)
println("Integrator created for bilinear dynamics")Integrator created for bilinear dynamicsStep 4: Define the Objective
Minimize control effort:
obj = QuadraticRegularizer(:u, traj, 1.0)
println("Objective: minimize ∫ ||u||² dt")Objective: minimize ∫ ||u||² dtStep 5: Create and Solve the Problem
Assemble the optimization problem:
prob = DirectTrajOptProblem(traj, obj, integrator)
probDirectTrajOptProblem
Trajectory
Timesteps: 50
Duration: 4.9
Knot dim: 4
Variables: x (2), u (1), Δt (1)
Controls: u, Δt
Objective: QuadraticRegularizer on :u (R = [1.0], all)
Dynamics (1 integrators)
BilinearIntegrator: :x = exp(Δt G(:u)) :x (dim = 2)
Constraints (3 total: 2 equality, 1 bounds)
EqualityConstraint: "initial value of x"
EqualityConstraint: "final value of x"
BoundsConstraint: "bounds on Δt"println("Solving optimization problem...")
println("="^50)Solving optimization problem...
==================================================Solve with Ipopt:
solve!(prob; max_iter = 100, verbose = false)
println("="^50)
println("Optimization complete!\n")This is Ipopt version 3.14.19, running with linear solver MUMPS 5.8.2.
Number of nonzeros in equality constraint Jacobian...: 776
Number of nonzeros in inequality constraint Jacobian.: 0
Number of nonzeros in Lagrangian Hessian.............: 1254
Total number of variables............................: 196
variables with only lower bounds: 50
variables with lower and upper bounds: 0
variables with only upper bounds: 0
Total number of equality constraints.................: 98
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 0.0000000e+00 9.68e-02 1.00e+00 0.0 0.00e+00 - 0.00e+00 0.00e+00 0
1 4.4571463e-05 3.38e-02 1.68e+00 -4.0 3.37e-01 0.0 9.90e-01 1.00e+00h 1
2 3.4481713e-04 3.31e-02 7.62e+00 -0.7 4.91e+00 -0.5 9.43e-01 4.11e-02f 1
3 9.7296902e-03 3.79e-02 9.30e+01 -0.6 3.07e+00 0.9 1.00e+00 9.19e-02h 1
4 7.3478995e-02 3.54e-02 6.30e+02 1.0 4.55e+00 2.2 3.94e-01 9.66e-02f 1
5 1.5842265e-01 4.28e-02 3.82e+03 1.8 1.83e+01 2.6 8.95e-01 1.17e-02f 1
6 2.4223910e+00 2.05e-01 9.15e+03 2.0 2.08e+00 3.0 5.36e-01 5.09e-01f 1
7 4.0931864e+00 2.77e-01 1.93e+03 1.9 2.90e-01 3.5 9.96e-01 1.00e+00f 1
8 5.0848449e+00 7.75e-01 3.23e+03 1.5 5.13e-01 3.0 1.00e+00 1.00e+00f 1
9 1.9974198e+01 8.75e-01 5.07e+05 1.2 2.11e+00 2.5 1.85e-01 1.00e+00f 1
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
10 1.5747239e+01 3.09e-01 1.24e+05 1.2 2.87e-01 2.9 4.91e-01 1.00e+00f 1
Number of Iterations....: 11
Number of objective function evaluations = 12
Number of objective gradient evaluations = 12
Number of equality constraint evaluations = 12
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 11
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 11
Total seconds in IPOPT = 6.331
EXIT: Invalid number in NLP function or derivative detected.
==================================================
Optimization complete!Step 6: Analyze the Solution
Extract the solution:
x_sol = prob.trajectory.x
u_sol = prob.trajectory.u
times = cumsum([0.0; prob.trajectory.Δt[:]])
println("Solution analysis:")
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))Solution analysis:
Initial state: [0.0, 0.0]
Final state: [1.0, 0.0]
Goal state: [1.0, 0.0]
Final error: 0.0Control statistics:
u_norm = norm(u_sol)
u_max = maximum(abs.(u_sol))
u_mean = mean(abs.(u_sol))
println("\nControl statistics:")
println(" Total norm: ", u_norm)
println(" Max magnitude: ", u_max)
println(" Mean magnitude: ", u_mean)
Control statistics:
Total norm: 5.80718099079191
Max magnitude: 2.5565744592543744
Mean magnitude: 0.5449745893740776Step 7: Verify Dynamics
Check that the solution satisfies the dynamics at a few points:
function verify_dynamics(x, u, Δt, G, k)
# Compute x[k+1] using the dynamics
x_k = x[:, k]
u_k = u[:, k]
Δt_k = Δt[k]
# Matrix exponential integration
x_k1_predicted = exp(Δt_k * G(u_k)) * x_k
x_k1_actual = x[:, k+1]
error = norm(x_k1_predicted - x_k1_actual)
return error
end
println("\nDynamics verification (error at selected time steps):")
for k in [1, 10, 25, 40, N-1]
error = verify_dynamics(x_sol, u_sol, prob.trajectory.Δt, G, k)
println(" k=$k: error = ", error)
end
Dynamics verification (error at selected time steps):
k=1: error = 7.174638823493076e-24
k=10: error = 1.136446216995715e-8
k=25: error = 3.4854612500358404e-5
k=40: error = 0.0017403212376633868
k=49: error = 0.0003159297621668843Visualization (Conceptual)
In a Jupyter notebook or with plotting packages, you could visualize:
println("\n" * "="^50)
println("SOLUTION SUMMARY")
println("="^50)
println("\nState trajectory (first 10 and last 10 time steps):")
println("Time | x₁ | x₂")
println("-"^25)
for k in [1:10; (N-9):N]
t = times[k]
println(@sprintf("%.2f | %7.4f | %7.4f", t, x_sol[1, k], x_sol[2, k]))
end
println("\nControl trajectory (first 10 and last 10 time steps):")
println("Time | u")
println("-"^15)
for k in [1:10; (N-9):N]
t = times[k]
println(@sprintf("%.2f | %7.4f", t, u_sol[1, k]))
end
==================================================
SOLUTION SUMMARY
==================================================
State trajectory (first 10 and last 10 time steps):
Time | x₁ | x₂
-------------------------
0.00 | 0.0000 | 0.0000
0.72 | -0.0000 | -0.0000
1.37 | 0.0000 | 0.0000
2.02 | -0.0000 | 0.0000
2.65 | 0.0000 | 0.0000
3.27 | -0.0000 | 0.0000
3.89 | 0.0000 | -0.0000
4.48 | -0.0000 | -0.0000
5.07 | -0.0000 | -0.0000
5.63 | -0.0000 | 0.0000
18.93 | 0.9390 | 0.2197
19.01 | 0.9583 | 0.1965
19.08 | 0.9732 | 0.1727
19.16 | 0.9864 | 0.1498
19.24 | 0.9986 | 0.1288
19.31 | 1.0041 | 0.1047
19.35 | 1.0051 | 0.0788
19.38 | 1.0043 | 0.0524
19.41 | 1.0026 | 0.0260
19.44 | 1.0000 | 0.0000
Control trajectory (first 10 and last 10 time steps):
Time | u
---------------
0.00 | 0.0000
0.72 | 0.0079
1.37 | 0.0124
2.02 | 0.0134
2.65 | 0.0118
3.27 | 0.0093
3.89 | 0.0078
4.48 | 0.0096
5.07 | 0.0159
5.63 | 0.0270
18.93 | 0.7209
19.01 | 0.6966
19.08 | 0.7403
19.16 | 0.8091
19.24 | 0.6640
19.31 | 0.4109
19.35 | 0.1673
19.38 | 0.0285
19.41 | 0.1618
19.44 | -0.0000Key Takeaways
- Linear interpolation provides a good initial guess for smooth problems
- BilinearIntegrator handles control-linear dynamics exactly
- Boundary conditions (initial/final) are enforced as hard constraints
- Control effort minimization produces smooth, efficient controls
- The solver finds a solution that satisfies dynamics and reaches the goal
Exercises
Try modifying the problem:
Exercise 1: Change the goal
Try reaching x_goal = [0.5, 0.5] instead
Exercise 2: Add control bounds
Limit the control: -1.0 ≤ u ≤ 1.0
traj_bounded = 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,)
)Exercise 3: Vary the control weight
Try QuadraticRegularizer(:u, traj, 0.1) (less penalty) or QuadraticRegularizer(:u, traj, 10.0) (more penalty)
Exercise 4: Add a terminal cost
Use soft goal constraint instead:
traj_soft = NamedTrajectory(
(x = x_guess, u = u_guess, Δt = fill(Δt, N));
timestep=:Δt,
controls=:u,
initial=(x = x_init,) # No final constraint
)
obj_soft = QuadraticRegularizer(:u, traj_soft, 1.0) +
TerminalObjective(x -> 100.0 * norm(x - x_goal)^2, :x, traj_soft)Next Steps
- Bilinear Control Tutorial: Multiple drives and bounds
- Minimum Time Tutorial: Optimize trajectory duration
- Smooth Controls Tutorial: Add derivative penalties
This page was generated using Literate.jl.