[skip ci] working on error in example

CLeARoboticsLab · Nov 25, 2024 · 9d839bd · 9d839bd
1 parent f144c16
commit 9d839bd
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 4 deletions.
diff --git a/examples/utils.jl b/examples/utils.jl
@@ -177,6 +177,7 @@ function TrajectoryGamesBase.solve_trajectory_game!(
 )
     # Solve, maybe with warm starting.
     if !isnothing(strategy.last_solution) && strategy.last_solution.status == :solved
+        println("warm start")
         solution = MCPSolver.solve(
             parametric_game,
             parameter_value;
@@ -185,7 +186,9 @@ function TrajectoryGamesBase.solve_trajectory_game!(
             y₀ = strategy.last_solution.variables.y,
         )
     else
+        println("cold start")
         (; initial_state) = unpack_parameters(parameter_value; game.dynamics)
+        @infiltrate
         solution = MCPSolver.solve(
             parametric_game,
             parameter_value;

diff --git a/src/game.jl b/src/game.jl
@@ -80,8 +80,8 @@ function ParametricGame(;
             [
                 reduce(vcat, ∇Ls)
                 reduce(vcat, gs)
-                reduce(vcat, hs)
                 g̃
+                reduce(vcat, hs)
                 h̃
             ],
         ),
@@ -93,8 +93,8 @@ function ParametricGame(;
             [
                 x
                 mapreduce(b -> length(b) == 0 ? nothing : b, vcat, blocks(λ))
-                mapreduce(b -> length(b) == 0 ? nothing : b, vcat, blocks(μ))
                 length(λ̃) == 0 ? nothing : λ̃
+                mapreduce(b -> length(b) == 0 ? nothing : b, vcat, blocks(μ))
                 length(μ̃) == 0 ? nothing : μ̃
             ],
         ),
@@ -103,16 +103,16 @@ function ParametricGame(;
     z̲ = [
         fill(-Inf, length(x))
         fill(-Inf, length(λ))
-        fill(0, length(μ))
         fill(-Inf, length(λ̃))
+        fill(0, length(μ))
         fill(0, length(μ̃))
     ]
 
     z̅ = [
         fill(Inf, length(x))
         fill(Inf, length(λ))
-        fill(Inf, length(μ))
         fill(Inf, length(λ̃))
+        fill(Inf, length(μ))
         fill(Inf, length(μ̃))
     ]
 
@@ -156,6 +156,7 @@ function solve(
     y₀ = ones(sum(game.dims.μ) + game.dims.μ̃),
     tol = 1e-4,
 )
+    @infiltrate
     (; x, y, s, kkt_error, status) = solve(solver_type, game.mcp; θ, x₀, y₀, tol)
 
     # Unpack primals per-player for ease of access later.

diff --git a/src/solver.jl b/src/solver.jl
@@ -38,8 +38,11 @@ function solve(
         while kkt_error > ϵ && iters < max_inner_iters
             # Compute the Newton step.
             # TODO! Can add some adaptive regularization.
+            @infiltrate
             F = mcp.F(x, y, s; θ, ϵ)
+            @infiltrate
             δz = -mcp.∇F(x, y, s; θ, ϵ) \ F
+            @infiltrate
 
             # Fraction to the boundary linesearch.
             δx = @view δz[1:(mcp.unconstrained_dimension)]