adding parameterization to mcp

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

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

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

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)]

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