diff --git a/benchmark/Project.toml b/benchmark/Project.toml index b781345..6b3da6e 100644 --- a/benchmark/Project.toml +++ b/benchmark/Project.toml @@ -1,11 +1,16 @@ [deps] +BlockArrays = "8e7c35d0-a365-5155-bbbb-fb81a777f24e" Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f" +LazySets = "b4f0291d-fe17-52bc-9479-3d1a343d9043" +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" MixedComplementarityProblems = "6c9e26cb-9263-41b8-a6c6-f4ca104ccdcd" PATHSolver = "f5f7c340-0bb3-5c69-969a-41884d311d1b" ParametricMCPs = "9b992ff8-05bb-4ea1-b9d2-5ef72d82f7ad" ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" +TrajectoryGamesBase = "ac1ac542-73eb-4349-ae1b-660ab3609574" +TrajectoryGamesExamples = "ff3fa34c-8d8f-519c-b5bc-31760c52507a" [sources] MixedComplementarityProblems = {path = ".."} diff --git a/benchmark/README.md b/benchmark/README.md index 6d2f7b3..f29d1ff 100644 --- a/benchmark/README.md +++ b/benchmark/README.md @@ -8,6 +8,13 @@ Currently, this directory provides code to benchmark the `InteriorPoint` solver ```julia julia> Revise.includet("SolverBenchmarks.jl") -julia> data = SolverBenchmarks.benchmark() +julia> data = SolverBenchmarks.benchmark(SolverBenchmarks.TrajectoryGameBenchmark(); num_samples = 25) julia> SolverBenchmarks.summary_statistics(data) -``` \ No newline at end of file +``` + +If you want to re-run with different kwargs, you may be able to reuse the MCPs and avoid waiting for them to compile: + +```julia +julia> data = SolverBenchmarks.benchmark(SolverBenchmarks.TrajectoryGameBenchmark(); num_samples = 250, data.ip_mcp, data.path_mcp) +julia> SolverBenchmarks.summary_statistics(data) +``` diff --git a/benchmark/SolverBenchmarks.jl b/benchmark/SolverBenchmarks.jl index a93b40a..c69e252 100644 --- a/benchmark/SolverBenchmarks.jl +++ b/benchmark/SolverBenchmarks.jl @@ -3,12 +3,20 @@ module SolverBenchmarks using MixedComplementarityProblems: MixedComplementarityProblems using ParametricMCPs: ParametricMCPs +using BlockArrays: BlockArrays, mortar using Random: Random using Statistics: Statistics using Distributions: Distributions +using LazySets: LazySets using PATHSolver: PATHSolver using ProgressMeter: @showprogress +abstract type BenchmarkType end +struct QuadraticProgramBenchmark <: BenchmarkType end +struct TrajectoryGameBenchmark <: BenchmarkType end + +include("quadratic_program_benchmark.jl") +include("trajectory_game_benchmark.jl") include("path.jl") end # module SolverBenchmarks diff --git a/benchmark/path.jl b/benchmark/path.jl index 335b43e..8158f0b 100644 --- a/benchmark/path.jl +++ b/benchmark/path.jl @@ -1,113 +1,66 @@ -""" Generate a random large (convex) quadratic problem of the form - min_x 0.5 xᵀ M x - ϕᵀ x - s.t. Ax - b ≥ 0. - -NOTE: the problem may not be feasible! -""" -function generate_test_problem(; num_primals, num_inequalities) - G(x, y; θ) = - let - (; M, A, ϕ) = unpack_parameters(θ; num_primals, num_inequalities) - M * x - ϕ - A' * y - end - - H(x, y; θ) = - let - (; A, b) = unpack_parameters(θ; num_primals, num_inequalities) - A * x - b - end - - K(z, θ) = - let - x = z[1:num_primals] - y = z[(num_primals + 1):end] - - [G(x, y; θ); H(x, y; θ)] - end - - (; G, H, K) -end - -"Generate a random parameter vector Θ corresponding to a convex QP." -function generate_random_parameter(rng; num_primals, num_inequalities, sparsity_rate) - bernoulli = Distributions.Bernoulli(1 - sparsity_rate) - - M = let - P = - randn(rng, num_primals, num_primals) .* - rand(rng, bernoulli, num_primals, num_primals) - P' * P - end - - A = - randn(rng, num_inequalities, num_primals) .* - rand(rng, bernoulli, num_inequalities, num_primals) - b = randn(rng, num_inequalities) - ϕ = randn(rng, num_primals) - - [reshape(M, length(M)); reshape(A, length(A)); b; ϕ] -end - -"Unpack a parameter vector θ into the components of a convex QP." -function unpack_parameters(θ; num_primals, num_inequalities) - M = reshape(θ[1:(num_primals^2)], num_primals, num_primals) - A = reshape( - θ[(num_primals^2 + 1):(num_primals^2 + num_inequalities * num_primals)], - num_inequalities, - num_primals, - ) - - b = - θ[(num_primals^2 + num_inequalities * num_primals + 1):(num_primals^2 + num_inequalities * (num_primals + 1))] - ϕ = θ[(num_primals^2 + num_inequalities * (num_primals + 1) + 1):end] - - (; M, A, b, ϕ) -end - -"Benchmark interior point solver against PATH on a bunch of random QPs." -function benchmark(; - num_samples = 1000, - num_primals = 100, - num_inequalities = 100, - sparsity_rate = 0.9, +"Benchmark interior point solver against PATH on a bunch of random test problems." +function benchmark( + benchmark_type; + num_samples = 100, + problem_kwargs = (;), ip_mcp = nothing, path_mcp = nothing, ip_kwargs = (; tol = 1e-6), ) - rng = Random.MersenneTwister(1) - # Generate problem and random parameters. @info "Generating random problems..." - problem = generate_test_problem(; num_primals, num_inequalities) + problem = generate_test_problem(benchmark_type; problem_kwargs...) + rng = Random.MersenneTwister(1) θs = map(1:num_samples) do _ - generate_random_parameter(rng; num_primals, num_inequalities, sparsity_rate) + generate_random_parameter(benchmark_type; rng, problem_kwargs...) end # Generate corresponding MCPs. @info "Generating IP MCP..." parameter_dimension = length(first(θs)) - ip_mcp = - !isnothing(ip_mcp) ? ip_mcp : + ip_mcp = if !isnothing(ip_mcp) + ip_mcp + elseif hasproperty(problem, :K) + # Generated a callable problem. MixedComplementarityProblems.PrimalDualMCP( - problem.G, - problem.H; - unconstrained_dimension = num_primals, - constrained_dimension = num_inequalities, + problem.K, + problem.lower_bounds, + problem.upper_bounds; parameter_dimension, ) + else + # Generated a symbolic problem. + MixedComplementarityProblems.PrimalDualMCP( + problem.K_symbolic, + problem.z_symbolic, + problem.θ_symbolic, + problem.lower_bounds, + problem.upper_bounds, + ) + end @info "Generating PATH MCP..." - lower_bounds = [fill(-Inf, num_primals); fill(0, num_inequalities)] - upper_bounds = fill(Inf, num_primals + num_inequalities) - path_mcp = - !isnothing(path_mcp) ? path_mcp : + path_mcp = if !isnothing(path_mcp) + path_mcp + elseif hasproperty(problem, :K) + # Generated a callable problem. ParametricMCPs.ParametricMCP( problem.K, - lower_bounds, - upper_bounds, + problem.lower_bounds, + problem.upper_bounds, parameter_dimension, ) + else + # Generated a symbolic problem. + ParametricMCPs.ParametricMCP( + problem.K_symbolic, + problem.z_symbolic, + problem.θ_symbolic, + problem.lower_bounds, + problem.upper_bounds + ) + end # Warm up the solvers. @info "Warming up IP solver..." @@ -127,7 +80,7 @@ function benchmark(; MixedComplementarityProblems.InteriorPoint(), ip_mcp, θ; - ip_kwargs... + ip_kwargs..., ) (; elapsed_time, success = sol.status == :solved) @@ -150,7 +103,8 @@ function summary_statistics(data) (; success_rate = fraction_solved(solver_data), runtime_stats(solver_data)...) end - stats = (; ip = accumulate_stats(data.ip_data), path = accumulate_stats(data.path_data)) + stats = + (; ip = accumulate_stats(data.ip_data), path = accumulate_stats(data.path_data)) @info "IP runtime is $(100(stats.ip.μ / stats.path.μ)) % that of PATH." stats diff --git a/benchmark/quadratic_program_benchmark.jl b/benchmark/quadratic_program_benchmark.jl new file mode 100644 index 0000000..63fd27b --- /dev/null +++ b/benchmark/quadratic_program_benchmark.jl @@ -0,0 +1,90 @@ +""" Generate a random (convex) quadratic problem of the form + min_x 0.5 xᵀ M x - ϕᵀ x + s.t. Ax - b ≥ 0. + +NOTE: the problem may not be feasible! +""" +function generate_test_problem( + ::QuadraticProgramBenchmark; + num_primals = 100, + num_inequalities = 100, +) + G(x, y; θ) = + let + (; M, A, ϕ) = unpack_parameters( + QuadraticProgramBenchmark(), + θ; + num_primals, + num_inequalities, + ) + M * x - ϕ - A' * y + end + + H(x, y; θ) = + let + (; A, b) = unpack_parameters( + QuadraticProgramBenchmark(), + θ; + num_primals, + num_inequalities, + ) + A * x - b + end + + K(z, θ) = + let + x = z[1:num_primals] + y = z[(num_primals + 1):end] + + [G(x, y; θ); H(x, y; θ)] + end + + unconstrained_dimension = num_primals + constrained_dimension = num_inequalities + lower_bounds = [fill(-Inf, num_primals); fill(0, num_inequalities)] + upper_bounds = fill(Inf, num_primals + num_inequalities) + + (; K, lower_bounds, upper_bounds) +end + +"Generate a random parameter vector Θ corresponding to a convex QP." +function generate_random_parameter( + ::QuadraticProgramBenchmark; + rng, + num_primals = 100, + num_inequalities = 100, + sparsity_rate = 0.9, +) + bernoulli = Distributions.Bernoulli(1 - sparsity_rate) + + M = let + P = + randn(rng, num_primals, num_primals) .* + rand(rng, bernoulli, num_primals, num_primals) + P' * P + end + + A = + randn(rng, num_inequalities, num_primals) .* + rand(rng, bernoulli, num_inequalities, num_primals) + b = randn(rng, num_inequalities) + ϕ = randn(rng, num_primals) + + [reshape(M, length(M)); reshape(A, length(A)); b; ϕ] +end + +"Unpack a parameter vector θ into the components of a convex QP." +function unpack_parameters(::QuadraticProgramBenchmark, θ; num_primals, num_inequalities) + M = reshape(θ[1:(num_primals^2)], num_primals, num_primals) + A = reshape( + θ[(num_primals^2 + 1):(num_primals^2 + num_inequalities * num_primals)], + num_inequalities, + num_primals, + ) + + b = + θ[(num_primals^2 + num_inequalities * num_primals + 1):(num_primals^2 + num_inequalities * (num_primals + 1))] + ϕ = θ[(num_primals^2 + num_inequalities * (num_primals + 1) + 1):end] + + (; M, A, b, ϕ) +end diff --git a/benchmark/trajectory_game_benchmark.jl b/benchmark/trajectory_game_benchmark.jl new file mode 100644 index 0000000..7e2bd1b --- /dev/null +++ b/benchmark/trajectory_game_benchmark.jl @@ -0,0 +1,87 @@ +module TrajectoryGameBenchmarkUtils + +using MixedComplementarityProblems: MixedComplementarityProblems + +using LazySets: LazySets +using TrajectoryGamesBase: + TrajectoryGamesBase, + PolygonEnvironment, + ProductDynamics, + TimeSeparableTrajectoryGameCost, + TrajectoryGame, + GeneralSumCostStructure, + num_players, + time_invariant_linear_dynamics, + unstack_trajectory, + stack_trajectories, + state_dim, + control_dim, + state_bounds, + control_bounds, + OpenLoopStrategy, + JointStrategy, + RecedingHorizonStrategy, + rollout +using TrajectoryGamesExamples: planar_double_integrator, animate_sim_steps +using BlockArrays: mortar, blocks, BlockArray, Block +using LinearAlgebra: norm_sqr, norm +using ProgressMeter: ProgressMeter + +include("../examples/utils.jl") +include("../examples/lane_change.jl") + +end # module TrajectoryGameBenchmarkUtils + +"Generate a random trajectory game, based on the `LaneChange` problem in `examples/`." +function generate_test_problem( + ::TrajectoryGameBenchmark; + horizon = 10, + height = 50, + num_lanes = 2, + lane_width = 2, +) + (; environment) = TrajectoryGameBenchmarkUtils.setup_road_environment(; + num_lanes, + lane_width, + height, + ) + game = TrajectoryGameBenchmarkUtils.setup_trajectory_game(; environment) + + # Build a game. Each player has a parameter for lane preference. P1 wants to stay + # in the left lane, and P2 wants to move from the right to the left lane. + TrajectoryGameBenchmarkUtils.build_mcp_components(; + game, + horizon, + params_per_player = 1, + ) +end + +""" Generate a random parameter vector Θ corresponding to an initial state and +horizontal tracking reference per player. +""" +function generate_random_parameter( + ::TrajectoryGameBenchmark; + rng, + num_lanes = 2, + lane_width = 2, + height = 50, +) + (; environment, lane_centers) = TrajectoryGameBenchmarkUtils.setup_road_environment(; + num_lanes, + lane_width, + height, + ) + + initial_states = mortar([ + [LazySets.sample(environment.set; rng); zeros(2)], + [LazySets.sample(environment.set; rng); zeros(2)], + ]) + horizontal_references = mortar([[rand(rng, lane_centers)], [rand(rng, lane_centers)]]) + + collect( + TrajectoryGameBenchmarkUtils.pack_parameters( + initial_states, + horizontal_references, + ), + ) +end diff --git a/examples/TrajectoryExamples.jl b/examples/TrajectoryExamples.jl index b799aab..c7e7bfc 100644 --- a/examples/TrajectoryExamples.jl +++ b/examples/TrajectoryExamples.jl @@ -44,6 +44,21 @@ using Makie: Makie using LinearAlgebra: norm_sqr, norm using ProgressMeter: ProgressMeter +"Visualize a strategy `γ` on a makie canvas using the base color `color`." +function TrajectoryGamesBase.visualize!( + canvas, + γ::Makie.Observable{<:OpenLoopStrategy}; + color = :black, + weight_offset = 0.0, +) + Makie.series!(canvas, γ; color = [(color, min(1.0, 0.9 + weight_offset))]) +end + +function Makie.convert_arguments(::Type{<:Makie.Series}, γ::OpenLoopStrategy) + traj_points = map(s -> Makie.Point2f(s[1:2]), γ.xs) + ([traj_points],) +end + include("utils.jl") include("lane_change.jl") diff --git a/examples/utils.jl b/examples/utils.jl index 97e8a50..99c95fe 100644 --- a/examples/utils.jl +++ b/examples/utils.jl @@ -54,6 +54,40 @@ function build_parametric_game(; game::TrajectoryGame, horizon = 10, params_per_player = 0, # not including initial state, which is always a param +) + (; + K_symbolic, + z_symbolic, + θ_symbolic, + lower_bounds, + upper_bounds, + dims, + problems, + shared_equality, + shared_inequality, + ) = build_mcp_components(; game, horizon, params_per_player) + + mcp = MixedComplementarityProblems.PrimalDualMCP( + K_symbolic, + z_symbolic, + θ_symbolic, + lower_bounds, + upper_bounds, + ) + MixedComplementarityProblems.ParametricGame( + problems, + shared_equality, + shared_inequality, + dims, + mcp, + ) +end + +"Construct MCP components from game components." +function build_mcp_components(; + game::TrajectoryGame, + horizon = 10, + params_per_player = 0, # not including initial state, which is always a param ) N = 2 N == num_players(game) || error("Should have only two players.") @@ -125,15 +159,22 @@ function build_parametric_game(; ii in 1:N ] - MixedComplementarityProblems.ParametricGame(; + problems = map( + f -> MixedComplementarityProblems.OptimizationProblem(; objective = f), + objectives, + ) + + components = MixedComplementarityProblems.game_to_mcp(; test_point = BlockArray(zeros(sum(primal_dims)), primal_dims), test_parameter = mortar([ zeros(state_dim(game.dynamics, ii) + params_per_player) for ii in 1:N ]), - problems = map(f -> MixedComplementarityProblems.OptimizationProblem(; objective = f), objectives), + problems, shared_equality, shared_inequality, ) + + (; problems, shared_equality, shared_inequality, components...) end "Generate an initial guess for primal variables following a zero input sequence." @@ -230,18 +271,3 @@ function (strategy::WarmStartRecedingHorizonStrategy)(state, time) strategy.receding_horizon_strategy(state, time_along_plan) end - -"Visualize a strategy `γ` on a makie canvas using the base color `color`." -function TrajectoryGamesBase.visualize!( - canvas, - γ::Makie.Observable{<:OpenLoopStrategy}; - color = :black, - weight_offset = 0.0, -) - Makie.series!(canvas, γ; color = [(color, min(1.0, 0.9 + weight_offset))]) -end - -function Makie.convert_arguments(::Type{<:Makie.Series}, γ::OpenLoopStrategy) - traj_points = map(s -> Makie.Point2f(s[1:2]), γ.xs) - ([traj_points],) -end diff --git a/src/game.jl b/src/game.jl index 71dff34..a6279f0 100644 --- a/src/game.jl +++ b/src/game.jl @@ -29,6 +29,27 @@ function ParametricGame(; problems, shared_equality = nothing, shared_inequality = nothing, +) + (; K_symbolic, z_symbolic, θ_symbolic, lower_bounds, upper_bounds, dims) = + game_to_mcp(; + test_point, + test_parameter, + problems, + shared_equality, + shared_inequality, + ) + + mcp = PrimalDualMCP(K_symbolic, z_symbolic, θ_symbolic, lower_bounds, upper_bounds) + ParametricGame(problems, shared_equality, shared_inequality, dims, mcp) +end + +"Helper for converting game components to MCP components." +function game_to_mcp(; + test_point, + test_parameter, + problems, + shared_equality = nothing, + shared_inequality = nothing, ) N = length(problems) @assert N == length(blocks(test_point)) @@ -83,7 +104,7 @@ function ParametricGame(; # Build MCP representation. symbolic_type = eltype(x) - F = Vector{symbolic_type}( + K = Vector{symbolic_type}( filter!( !isnothing, [ @@ -109,7 +130,7 @@ function ParametricGame(; ), ) - z̲ = [ + lower_bounds = [ fill(-Inf, length(x)) fill(-Inf, length(λ)) fill(-Inf, length(λ̃)) @@ -117,7 +138,7 @@ function ParametricGame(; fill(0, length(μ̃)) ] - z̅ = [ + upper_bounds = [ fill(Inf, length(x)) fill(Inf, length(λ)) fill(Inf, length(λ̃)) @@ -125,9 +146,14 @@ function ParametricGame(; fill(Inf, length(μ̃)) ] - mcp = PrimalDualMCP(F |> collect, z |> collect, θ |> collect, z̲, z̅) - - ParametricGame(problems, shared_equality, shared_inequality, dims, mcp) + (; + K_symbolic = collect(K), + z_symbolic = collect(z), + θ_symbolic = collect(θ), + lower_bounds, + upper_bounds, + dims, + ) end function dimensions(