From 1fb6f55a94a5cd4493db3e651ee8a23df9297dc9 Mon Sep 17 00:00:00 2001 From: David Fridovich-Keil Date: Wed, 20 Nov 2024 08:24:27 -0600 Subject: [PATCH] adding parameterization to mcp --- examples/lane_change.jl | 4 +-- examples/utils.jl | 4 +-- src/mcp.jl | 77 +++++++++++++++++++++++++---------------- src/solver.jl | 5 +-- test/runtests.jl | 26 +++++++------- 5 files changed, 68 insertions(+), 48 deletions(-) diff --git a/examples/lane_change.jl b/examples/lane_change.jl index 65278c0..9dac2ac 100644 --- a/examples/lane_change.jl +++ b/examples/lane_change.jl @@ -96,8 +96,6 @@ function main(; num_lanes = 2, lane_width = 2, num_sim_steps = 150, - gradient_steps_per_turn = 5, - observation_noise_stddev = 0.1, ) (; environment, lane_centers) = setup_road_environment(; num_lanes, lane_width, height) @@ -107,7 +105,7 @@ function main(; # P1 wants to stay in the left lane, and P2 wants to move from the # right to the left lane. lane_preferences = mortar([[lane_centers[1]], [lane_centers[1]]]) - mcp = build_mcp(; game, horizon, params_per_player = 1) + parametric_game = build_parametric_game(; game, horizon, params_per_player = 1) # Simulate the ground truth. turn_length = 3 diff --git a/examples/utils.jl b/examples/utils.jl index 55f17e5..3bb81ed 100644 --- a/examples/utils.jl +++ b/examples/utils.jl @@ -251,7 +251,7 @@ end "Receding horizon strategy that supports warm starting." Base.@kwdef mutable struct WarmStartRecedingHorizonStrategy game::TrajectoryGame - mcp::PrimalDualMCP + parametric_game::ParametricGame receding_horizon_strategy::Any = nothing time_last_updated::Int = 0 turn_length::Int @@ -274,7 +274,7 @@ function (strategy::WarmStartRecedingHorizonStrategy)(state, time) strategy.horizon, pack_parameters(state, strategy.parameters), strategy; - strategy.mcp, + strategy.parametric_game, ) strategy.time_last_updated = time time_along_plan = 1 diff --git a/src/mcp.jl b/src/mcp.jl index f9a0883..7c61220 100644 --- a/src/mcp.jl +++ b/src/mcp.jl @@ -1,19 +1,19 @@ """ Store key elements of the primal-dual KKT system for a MCP composed of functions G(.) and H(.) such that - 0 = G(x, y) - 0 ≤ H(x, y) ⟂ y ≥ 0. + 0 = G(x, y; θ) + 0 ≤ H(x, y; θ) ⟂ y ≥ 0. The primal-dual system arises when we introduce slack variable `s` and set - G(x, y) = 0 - H(x, y) - s = 0 - s ⦿ y - ϵ = 0 -for some ϵ > 0. Define the function `F(z; ϵ)` to return the left hand side of this -system of equations, where `z = [x; y; s]`. + G(x, y; θ) = 0 + H(x, y; θ) - s = 0 + s ⦿ y - ϵ = 0 +for some ϵ > 0. Define the function `F(x, y, s; θ, ϵ)` to return the left +hand side of this system of equations. """ struct PrimalDualMCP{T1,T2} - "A callable `F(x, y, s; ϵ)` which computes the KKT error in the primal-dual system." + "A callable `F(x, y, s; θ, ϵ)` which computes the KKT error in the primal-dual system." F::T1 - "A callable `∇F(x, y, s; ϵ)` which stores the Jacobian of the KKT error wrt z." + "A callable `∇F(x, y, s; θ, ϵ)` which stores the Jacobian of the KKT error wrt z." ∇F::T2 "Dimension of unconstrained variable." unconstrained_dimension::Int @@ -22,24 +22,27 @@ struct PrimalDualMCP{T1,T2} end "Helper to construct a PrimalDualMCP from callable functions `G(.)` and `H(.)`." -function to_symbolic_mcp( +function PrimalDualMCP( G, H, unconstrained_dimension, - constrained_dimension; + constrained_dimension, + parameter_dimension; backend = SymbolicUtils.SymbolicsBackend(), backend_options = (;), ) x_symbolic = SymbolicUtils.make_variables(backend, :x, unconstrained_dimension) y_symbolic = SymbolicUtils.make_variables(backend, :y, constrained_dimension) - G_symbolic = G(x_symbolic, y_symbolic) - H_symbolic = H(x_symbolic, y_symbolic) + θ_symbolic = SymbolicUtils.make_variables(backend, :θ, parameter_dimension) + G_symbolic = G(x_symbolic, y_symbolic; θ = θ_symbolic) + H_symbolic = H(x_symbolic, y_symbolic; θ = θ_symbolic) PrimalDualMCP( G_symbolic, H_symbolic, x_symbolic, - y_symbolic; + y_symbolic, + θ_symbolic; backend, backend_options, ) @@ -50,7 +53,8 @@ function PrimalDualMCP( G_symbolic::Vector{T}, H_symbolic::Vector{T}, x_symbolic::Vector{T}, - y_symbolic::Vector{T}; + y_symbolic::Vector{T}, + θ_symbolic::Vector{T}; backend = SymbolicUtils.SymbolicsBackend(), backend_options = (;), ) where {T<:Union{FD.Node,Symbolics.Num}} @@ -68,53 +72,61 @@ function PrimalDualMCP( F = let _F = SymbolicUtils.build_function( F_symbolic, - [z_symbolic; ϵ_symbolic]; + [z_symbolic; θ_symbolic; ϵ_symbolic]; in_place = false, backend_options, ) - (x, y, s; ϵ) -> _F([x; y; s; ϵ]) + (x, y, s; θ, ϵ) -> _F([x; y; s; θ; ϵ]) end ∇F = let ∇F_symbolic = SymbolicUtils.sparse_jacobian(F_symbolic, z_symbolic) _∇F = SymbolicUtils.build_function( ∇F_symbolic, - [z_symbolic; ϵ_symbolic]; + [z_symbolic; θ_symbolic; ϵ_symbolic]; in_place = false, backend_options, ) - (x, y, s; ϵ) -> _∇F([x; y; s; ϵ]) + (x, y, s; θ, ϵ) -> _∇F([x; y; s; θ; ϵ]) end PrimalDualMCP(F, ∇F, length(x_symbolic), length(y_symbolic)) end -"""Construct a PrimalDualMCP from `K(z) ⟂ z̲ ≤ z ≤ z̅`, where `K` is callable. -NOTE: Assumes that all upper bounds are Inf, and lower bounds are either --Inf or 0. +"""Construct a PrimalDualMCP from `K(z; θ) ⟂ z̲ ≤ z ≤ z̅`, where `K` is callable. +NOTE: Assumes that all upper bounds are Inf, and lower bounds are either -Inf or 0. """ function PrimalDualMCP( K, lower_bounds::Vector, - upper_bounds::Vector; + upper_bounds::Vector, + parameter_dimension; backend = SymbolicUtils.SymbolicsBackend(), backend_options = (;), ) z_symbolic = SymbolicUtils.make_variables(backend, :z, length(lower_bounds)) - K_symbolic = K(z_symbolic) + θ_symbolic = SymbolicUtils.make_variables(backend, :θ, parameter_dimension) + K_symbolic = K(z_symbolic; θ = θ_symbolic) - PrimalDualMCP(K_symbolic, z_symbolic, lower_bounds, upper_bounds; backend_options) + PrimalDualMCP( + K_symbolic, + z_symbolic, + θ_symbolic, + lower_bounds, + upper_bounds; + backend_options, + ) end -"""Construct a PrimalDualMCP from symbolic `K(z) ⟂ z̲ ≤ z ≤ z̅`. -NOTE: Assumes that all upper bounds are Inf, and lower bounds are either --Inf or 0. +"""Construct a PrimalDualMCP from symbolic `K(z; θ) ⟂ z̲ ≤ z ≤ z̅`. +NOTE: Assumes that all upper bounds are Inf, and lower bounds are either -Inf or 0. """ function PrimalDualMCP( K_symbolic::Vector{T}, z_symbolic::Vector{T}, + θ_symbolic::Vector{T}, lower_bounds::Vector, upper_bounds::Vector; backend_options = (;), @@ -129,5 +141,12 @@ function PrimalDualMCP( x_symbolic = z_symbolic[unconstrained_indices] y_symbolic = z_symbolic[constrained_indices] - PrimalDualMCP(G_symbolic, H_symbolic, x_symbolic, y_symbolic; backend_options) + PrimalDualMCP( + G_symbolic, + H_symbolic, + x_symbolic, + y_symbolic, + θ_symbolic; + backend_options, + ) end diff --git a/src/solver.jl b/src/solver.jl index 88430eb..ac6014f 100644 --- a/src/solver.jl +++ b/src/solver.jl @@ -20,6 +20,7 @@ when the previous subproblem is solved in fewer iterations. function solve( ::InteriorPoint, mcp::PrimalDualMCP; + θ, x₀ = zeros(mcp.unconstrained_dimension), y₀ = ones(mcp.constrained_dimension), tol = 1e-4, @@ -35,8 +36,8 @@ function solve( while kkt_error > ϵ # Compute the Newton step. # TODO! Can add some adaptive regularization. - F = mcp.F(x, y, s; ϵ) - δz = -mcp.∇F(x, y, s; ϵ) \ F + F = mcp.F(x, y, s; θ, ϵ) + δz = -mcp.∇F(x, y, s; θ, ϵ) \ F # Fraction to the boundary linesearch. δx = @view δz[1:(mcp.unconstrained_dimension)] diff --git a/test/runtests.jl b/test/runtests.jl index e1b8e70..08b69de 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -13,39 +13,41 @@ using MCPSolver M = [2 1; 1 2] A = [1 0; 0 1] b = [1; 1] + θ = zeros(2) - G(x, y) = M * x - A' * y - H(x, y) = A * x - b - F(z) = begin + G(x, y; θ) = M * x - A' * y - θ + H(x, y; θ) = A * x - b + K(z; θ) = begin x = z[1:size(M, 1)] y = z[(size(M, 1) + 1):end] - [G(x, y); H(x, y)] + [G(x, y; θ); H(x, y; θ)] end function check_solution(sol) - @test all(abs.(G(sol.x, sol.y)) .≤ 1e-3) - @test all(H(sol.x, sol.y) .≥ 0) + @test all(abs.(G(sol.x, sol.y; θ)) .≤ 1e-3) + @test all(H(sol.x, sol.y; θ) .≥ 0) @test all(sol.y .≥ 0) - @test sum(sol.y .* H(sol.x, sol.y)) ≤ 1e-3 + @test sum(sol.y .* H(sol.x, sol.y; θ)) ≤ 1e-3 @test all(sol.s .≤ 1e-3) @test sol.kkt_error ≤ 1e-3 end @testset "BasicCallableConstructor" begin - mcp = MCPSolver.to_symbolic_mcp(G, H, size(M, 1), length(b)) - sol = MCPSolver.solve(MCPSolver.InteriorPoint(), mcp) + mcp = MCPSolver.PrimalDualMCP(G, H, size(M, 1), length(b), size(M, 1)) + sol = MCPSolver.solve(MCPSolver.InteriorPoint(), mcp; θ) check_solution(sol) end @testset "AlternativeCallableConstructor" begin - mcp = MCPSolver.PrimalDualMCP( - F, + mcp = MCPSolver.PrimalDualMCP( + K, [fill(-Inf, size(M, 1)); fill(0, length(b))], fill(Inf, size(M, 1) + length(b)), + size(M, 1) ) - sol = MCPSolver.solve(MCPSolver.InteriorPoint(), mcp) + sol = MCPSolver.solve(MCPSolver.InteriorPoint(), mcp; θ) check_solution(sol) end