├── .JuliaFormatter.toml ├── .github ├── dependabot.yml └── workflows │ ├── tagbot.yml │ ├── compat.yml │ └── test.yml ├── test ├── Project.toml └── runtests.jl ├── examples ├── Project.toml ├── TrajectoryExamples.jl ├── lane_change.jl └── utils.jl ├── src ├── MixedComplementarityProblems.jl ├── AutoDiff.jl ├── solver.jl ├── game.jl └── mcp.jl ├── benchmark ├── SolverBenchmarks.jl ├── Project.toml ├── README.md ├── trajectory_game_benchmark.jl ├── quadratic_program_benchmark.jl └── path.jl ├── .gitignore ├── Project.toml ├── LICENSE └── README.md /.JuliaFormatter.toml: -------------------------------------------------------------------------------- 1 | margin = 90 2 | always_for_in = true 3 | import_to_using = true 4 | remove_extra_newlines = true 5 | whitespace_typedefs = false 6 | whitespace_ops_in_indices = true 7 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | # Set update schedule for GitHub Actions 2 | version: 2 3 | updates: 4 | - package-ecosystem: "github-actions" 5 | directory: "/" 6 | schedule: 7 | # Check for updates to GitHub Actions every week 8 | interval: "weekly" 9 | -------------------------------------------------------------------------------- /test/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | BlockArrays = "8e7c35d0-a365-5155-bbbb-fb81a777f24e" 3 | FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" 4 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 5 | Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 6 | Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 7 | Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" 8 | -------------------------------------------------------------------------------- /.github/workflows/tagbot.yml: -------------------------------------------------------------------------------- 1 | name: TagBot 2 | on: 3 | issue_comment: 4 | types: 5 | - created 6 | workflow_dispatch: 7 | inputs: 8 | lookback: 9 | default: 3 10 | permissions: 11 | contents: write 12 | jobs: 13 | TagBot: 14 | if: github.event_name == 'workflow_dispatch' || github.actor == 'JuliaTagBot' 15 | runs-on: ubuntu-latest 16 | steps: 17 | - uses: JuliaRegistries/TagBot@v1 18 | with: 19 | token: ${{ secrets.GITHUB_TOKEN }} 20 | ssh: ${{ secrets.GHACTION_PRIV }} 21 | -------------------------------------------------------------------------------- /examples/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | BlockArrays = "8e7c35d0-a365-5155-bbbb-fb81a777f24e" 3 | GLMakie = "e9467ef8-e4e7-5192-8a1a-b1aee30e663a" 4 | LazySets = "b4f0291d-fe17-52bc-9479-3d1a343d9043" 5 | LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 6 | Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a" 7 | MixedComplementarityProblems = "6c9e26cb-9263-41b8-a6c6-f4ca104ccdcd" 8 | ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca" 9 | TrajectoryGamesBase = "ac1ac542-73eb-4349-ae1b-660ab3609574" 10 | TrajectoryGamesExamples = "ff3fa34c-8d8f-519c-b5bc-31760c52507a" 11 | 12 | [sources] 13 | MixedComplementarityProblems = {path = ".."} 14 | -------------------------------------------------------------------------------- /.github/workflows/compat.yml: -------------------------------------------------------------------------------- 1 | name: CompatHelper 2 | on: 3 | schedule: 4 | - cron: '00 00 * * *' 5 | push: 6 | branches: [ main ] 7 | pull_request: 8 | branches: [ main ] 9 | issues: 10 | types: [opened, reopened] 11 | 12 | jobs: 13 | CompatHelper: 14 | runs-on: ubuntu-latest 15 | steps: 16 | - name: Pkg.add("CompatHelper") 17 | run: julia -e 'using Pkg; Pkg.add("CompatHelper")' 18 | - name: CompatHelper.main() 19 | env: 20 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 21 | COMPATHELPER_PRIV: ${{ secrets.GHACTION_PRIV }} 22 | run: julia -e 'using CompatHelper; CompatHelper.main()' 23 | -------------------------------------------------------------------------------- /src/MixedComplementarityProblems.jl: -------------------------------------------------------------------------------- 1 | module MixedComplementarityProblems 2 | 3 | using SparseArrays: SparseArrays 4 | using LinearAlgebra: LinearAlgebra, I, norm, eigvals 5 | using BlockArrays: blocks, blocksizes 6 | using TrajectoryGamesBase: to_blockvector 7 | using SymbolicTracingUtils: SymbolicTracingUtils as SymbolicTracingUtils 8 | using LinearSolve: LinearProblem, init, solve!, KrylovJL_GMRES, UMFPACKFactorization 9 | using SciMLBase: SciMLBase 10 | 11 | include("mcp.jl") 12 | include("solver.jl") 13 | include("game.jl") 14 | include("AutoDiff.jl") 15 | 16 | export PrimalDualMCP, solve, ParametricGame, OptimizationProblem 17 | 18 | end # module MixedComplementarityProblems 19 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | name: test 2 | on: [push, pull_request] 3 | jobs: 4 | tests: 5 | name: test 6 | runs-on: ${{ matrix.os }} 7 | env: 8 | PATH_LICENSE_STRING: "2830898829&Courtesy&&&USR&45321&5_1_2021&1000&PATH&GEN&31_12_2025&0_0_0&6000&0_0" 9 | strategy: 10 | fail-fast: false 11 | matrix: 12 | os: 13 | - ubuntu-latest 14 | arch: 15 | - x64 16 | steps: 17 | - uses: actions/checkout@v6 18 | - uses: actions/checkout@v6 19 | - uses: julia-actions/setup-julia@v2 20 | - uses: julia-actions/cache@v2 21 | - uses: julia-actions/julia-runtest@v1 22 | with: 23 | prefix: "xvfb-run" 24 | -------------------------------------------------------------------------------- /benchmark/SolverBenchmarks.jl: -------------------------------------------------------------------------------- 1 | "Module for benchmarking different solvers against one another." 2 | module SolverBenchmarks 3 | 4 | using MixedComplementarityProblems: MixedComplementarityProblems 5 | using ParametricMCPs: ParametricMCPs 6 | using BlockArrays: BlockArrays, mortar 7 | using Random: Random 8 | using Statistics: Statistics 9 | using Distributions: Distributions 10 | using LazySets: LazySets 11 | using PATHSolver: PATHSolver 12 | using ProgressMeter: @showprogress 13 | using Symbolics: Symbolics 14 | 15 | abstract type BenchmarkType end 16 | struct QuadraticProgramBenchmark <: BenchmarkType end 17 | struct TrajectoryGameBenchmark <: BenchmarkType end 18 | 19 | include("quadratic_program_benchmark.jl") 20 | include("trajectory_game_benchmark.jl") 21 | include("path.jl") 22 | 23 | end # module SolverBenchmarks 24 | -------------------------------------------------------------------------------- /benchmark/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | BlockArrays = "8e7c35d0-a365-5155-bbbb-fb81a777f24e" 3 | Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f" 4 | LazySets = "b4f0291d-fe17-52bc-9479-3d1a343d9043" 5 | LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 6 | MixedComplementarityProblems = "6c9e26cb-9263-41b8-a6c6-f4ca104ccdcd" 7 | PATHSolver = "f5f7c340-0bb3-5c69-969a-41884d311d1b" 8 | ParametricMCPs = "9b992ff8-05bb-4ea1-b9d2-5ef72d82f7ad" 9 | ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca" 10 | Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 11 | Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 12 | Symbolics = "0c5d862f-8b57-4792-8d23-62f2024744c7" 13 | TrajectoryGamesBase = "ac1ac542-73eb-4349-ae1b-660ab3609574" 14 | TrajectoryGamesExamples = "ff3fa34c-8d8f-519c-b5bc-31760c52507a" 15 | 16 | [sources] 17 | MixedComplementarityProblems = {path = ".."} 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.mp4 2 | 3 | # Files generated by invoking Julia with --code-coverage 4 | *.jl.cov 5 | *.jl.*.cov 6 | 7 | # Files generated by invoking Julia with --track-allocation 8 | *.jl.mem 9 | 10 | # System-specific files and directories generated by the BinaryProvider and BinDeps packages 11 | # They contain absolute paths specific to the host computer, and so should not be committed 12 | deps/deps.jl 13 | deps/build.log 14 | deps/downloads/ 15 | deps/usr/ 16 | deps/src/ 17 | 18 | # Build artifacts for creating documentation generated by the Documenter package 19 | docs/build/ 20 | docs/site/ 21 | 22 | # File generated by Pkg, the package manager, based on a corresponding Project.toml 23 | # It records a fixed state of all packages used by the project. As such, it should not be 24 | # committed for packages, but should be committed for applications that require a static 25 | # environment. 26 | Manifest.toml 27 | -------------------------------------------------------------------------------- /benchmark/README.md: -------------------------------------------------------------------------------- 1 | # Solver Benchmarks 2 | 3 | Benchmarking `MixedComplementarityProblems` solver(s) against PATH. 4 | 5 | ## Instructions 6 | 7 | This directory provides code to benchmark the `InteriorPoint` solver against `PATH`, accessed via `ParametricMCPs` and `PATHSolver`. Currently, we provide two different benchmark problems: (i) a set of randomly-generated sparse quadratic programs with user-specified numbers of primal variables and inequality constraints, and (ii) the lane changing trajectory game from `examples/`, with initial conditions randomized. To run (with the REPL activated within this directory): 8 | 9 | ```julia 10 | julia> include("SolverBenchmarks.jl") 11 | julia> data = SolverBenchmarks.benchmark(SolverBenchmarks.TrajectoryGameBenchmark(); num_samples = 25); 12 | julia> SolverBenchmarks.summary_statistics(data) 13 | ``` 14 | 15 | If you want to re-run with different kwargs, you may be able to reuse the MCPs and avoid waiting for them to compile: 16 | 17 | ```julia 18 | julia> data = SolverBenchmarks.benchmark(SolverBenchmarks.TrajectoryGameBenchmark(); num_samples = 250, data.ip_mcp, data.path_mcp); 19 | julia> SolverBenchmarks.summary_statistics(data) 20 | ``` 21 | -------------------------------------------------------------------------------- /Project.toml: -------------------------------------------------------------------------------- 1 | name = "MixedComplementarityProblems" 2 | uuid = "6c9e26cb-9263-41b8-a6c6-f4ca104ccdcd" 3 | authors = ["David Fridovich-Keil "] 4 | version = "0.1.13" 5 | 6 | [deps] 7 | BlockArrays = "8e7c35d0-a365-5155-bbbb-fb81a777f24e" 8 | ChainRulesCore = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" 9 | DataStructures = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" 10 | FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" 11 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 12 | LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 13 | LinearSolve = "7ed4a6bd-45f5-4d41-b270-4a48e9bafcae" 14 | SciMLBase = "0bca4576-84f4-4d90-8ffe-ffa030f20462" 15 | SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 16 | SymbolicTracingUtils = "77ddf47f-b2ab-4ded-95ee-54f4fa148129" 17 | TrajectoryGamesBase = "ac1ac542-73eb-4349-ae1b-660ab3609574" 18 | Zygote = "e88e6eb3-aa80-5325-afca-941959d7151f" 19 | 20 | [compat] 21 | BlockArrays = "0.16.43, 1" 22 | ChainRulesCore = "1.25.0" 23 | DataStructures = "0.18.20" 24 | FiniteDiff = "2.26.2" 25 | ForwardDiff = "0.10.38, 1" 26 | LinearAlgebra = "1.11.0" 27 | LinearSolve = "2.38.0, 3" 28 | SciMLBase = "2.70.0" 29 | SparseArrays = "1.11.0" 30 | SymbolicTracingUtils = "0.1.1" 31 | TrajectoryGamesBase = "0.3.10" 32 | Zygote = "0.6.73, 0.7" 33 | julia = "1.11" 34 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, Control and Learning for Autonomous Robotics Lab 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /examples/TrajectoryExamples.jl: -------------------------------------------------------------------------------- 1 | """ 2 | Utilities for constructing trajectory games, in which each player wishes to 3 | solve a problem of the form: 4 | min_{τᵢ} fᵢ(τ, θ) 5 | 6 | where all vehicles must jointly satisfy the constraints 7 | g̃(τ, θ) = 0 8 | h̃(τ, θ) ≥ 0. 9 | 10 | Here, τᵢ is the ith vehicle's trajectory, consisting of states and controls. 11 | The shared constraints g̃ and h̃ incorporate dynamic feasibility, fixed initial 12 | condition, actuator and state limits, environment boundaries, and 13 | collision-avoidance. 14 | """ 15 | 16 | module TrajectoryExamples 17 | 18 | using MixedComplementarityProblems: MixedComplementarityProblems 19 | 20 | using LazySets: LazySets 21 | using TrajectoryGamesBase: 22 | TrajectoryGamesBase, 23 | PolygonEnvironment, 24 | ProductDynamics, 25 | TimeSeparableTrajectoryGameCost, 26 | TrajectoryGame, 27 | GeneralSumCostStructure, 28 | num_players, 29 | time_invariant_linear_dynamics, 30 | unstack_trajectory, 31 | stack_trajectories, 32 | state_dim, 33 | control_dim, 34 | state_bounds, 35 | control_bounds, 36 | OpenLoopStrategy, 37 | JointStrategy, 38 | RecedingHorizonStrategy, 39 | rollout 40 | using TrajectoryGamesExamples: planar_double_integrator, animate_sim_steps 41 | using BlockArrays: mortar, blocks, BlockArray, Block 42 | using GLMakie: GLMakie 43 | using Makie: Makie 44 | using LinearAlgebra: norm_sqr, norm 45 | using ProgressMeter: ProgressMeter 46 | 47 | "Visualize a strategy `γ` on a makie canvas using the base color `color`." 48 | function TrajectoryGamesBase.visualize!( 49 | canvas, 50 | γ::Makie.Observable{<:OpenLoopStrategy}; 51 | color = :black, 52 | weight_offset = 0.0, 53 | ) 54 | Makie.series!(canvas, γ; color = [(color, min(1.0, 0.9 + weight_offset))]) 55 | end 56 | 57 | function Makie.convert_arguments(::Type{<:Makie.Series}, γ::OpenLoopStrategy) 58 | traj_points = map(s -> Makie.Point2f(s[1:2]), γ.xs) 59 | ([traj_points],) 60 | end 61 | 62 | include("utils.jl") 63 | include("lane_change.jl") 64 | 65 | end # module TrajectoryExamples 66 | -------------------------------------------------------------------------------- /benchmark/trajectory_game_benchmark.jl: -------------------------------------------------------------------------------- 1 | module TrajectoryGameBenchmarkUtils 2 | 3 | using MixedComplementarityProblems: MixedComplementarityProblems 4 | 5 | using LazySets: LazySets 6 | using TrajectoryGamesBase: 7 | TrajectoryGamesBase, 8 | PolygonEnvironment, 9 | ProductDynamics, 10 | TimeSeparableTrajectoryGameCost, 11 | TrajectoryGame, 12 | GeneralSumCostStructure, 13 | num_players, 14 | time_invariant_linear_dynamics, 15 | unstack_trajectory, 16 | stack_trajectories, 17 | state_dim, 18 | control_dim, 19 | state_bounds, 20 | control_bounds, 21 | OpenLoopStrategy, 22 | JointStrategy, 23 | RecedingHorizonStrategy, 24 | rollout 25 | using TrajectoryGamesExamples: planar_double_integrator, animate_sim_steps 26 | using BlockArrays: mortar, blocks, BlockArray, Block 27 | using LinearAlgebra: norm_sqr, norm 28 | using ProgressMeter: ProgressMeter 29 | 30 | include("../examples/utils.jl") 31 | include("../examples/lane_change.jl") 32 | 33 | end # module TrajectoryGameBenchmarkUtils 34 | 35 | "Generate a random trajectory game, based on the `LaneChange` problem in `examples/`." 36 | function generate_test_problem( 37 | ::TrajectoryGameBenchmark; 38 | horizon = 10, 39 | height = 50, 40 | num_lanes = 2, 41 | lane_width = 2, 42 | ) 43 | (; environment) = TrajectoryGameBenchmarkUtils.setup_road_environment(; 44 | num_lanes, 45 | lane_width, 46 | height, 47 | ) 48 | game = TrajectoryGameBenchmarkUtils.setup_trajectory_game(; environment) 49 | 50 | # Build a game. Each player has a parameter for lane preference. P1 wants to stay 51 | # in the left lane, and P2 wants to move from the right to the left lane. 52 | TrajectoryGameBenchmarkUtils.build_mcp_components(; 53 | game, 54 | horizon, 55 | params_per_player = 1, 56 | ) 57 | end 58 | 59 | """ Generate a random parameter vector Θ corresponding to an initial state and 60 | horizontal tracking reference per player. 61 | """ 62 | function generate_random_parameter( 63 | ::TrajectoryGameBenchmark; 64 | rng, 65 | num_lanes = 2, 66 | lane_width = 2, 67 | height = 50, 68 | ) 69 | (; environment, lane_centers) = TrajectoryGameBenchmarkUtils.setup_road_environment(; 70 | num_lanes, 71 | lane_width, 72 | height, 73 | ) 74 | 75 | initial_states = mortar([ 76 | [LazySets.sample(environment.set; rng); zeros(2)], 77 | [LazySets.sample(environment.set; rng); zeros(2)], 78 | ]) 79 | horizontal_references = mortar([[rand(rng, lane_centers)], [rand(rng, lane_centers)]]) 80 | 81 | collect( 82 | TrajectoryGameBenchmarkUtils.pack_parameters( 83 | initial_states, 84 | horizontal_references, 85 | ), 86 | ) 87 | end 88 | -------------------------------------------------------------------------------- /benchmark/quadratic_program_benchmark.jl: -------------------------------------------------------------------------------- 1 | """ Generate a random (convex) quadratic problem of the form 2 | min_x 0.5 xᵀ M x - ϕᵀ x 3 | s.t. Ax - b ≥ 0. 4 | 5 | NOTE: the problem may not be feasible! 6 | """ 7 | function generate_test_problem( 8 | ::QuadraticProgramBenchmark; 9 | num_primals = 100, 10 | num_inequalities = 100, 11 | ) 12 | G(x, y; θ) = 13 | let 14 | (; M, A, ϕ) = unpack_parameters( 15 | QuadraticProgramBenchmark(), 16 | θ; 17 | num_primals, 18 | num_inequalities, 19 | ) 20 | M * x - ϕ - A' * y 21 | end 22 | 23 | H(x, y; θ) = 24 | let 25 | (; A, b) = unpack_parameters( 26 | QuadraticProgramBenchmark(), 27 | θ; 28 | num_primals, 29 | num_inequalities, 30 | ) 31 | A * x - b 32 | end 33 | 34 | K(z; θ) = 35 | let 36 | x = z[1:num_primals] 37 | y = z[(num_primals + 1):end] 38 | 39 | [G(x, y; θ); H(x, y; θ)] 40 | end 41 | 42 | unconstrained_dimension = num_primals 43 | constrained_dimension = num_inequalities 44 | lower_bounds = [fill(-Inf, num_primals); fill(0, num_inequalities)] 45 | upper_bounds = fill(Inf, num_primals + num_inequalities) 46 | 47 | (; K, lower_bounds, upper_bounds) 48 | end 49 | 50 | "Generate a random parameter vector Θ corresponding to a convex QP." 51 | function generate_random_parameter( 52 | ::QuadraticProgramBenchmark; 53 | rng, 54 | num_primals = 100, 55 | num_inequalities = 100, 56 | sparsity_rate = 0.9, 57 | ) 58 | bernoulli = Distributions.Bernoulli(1 - sparsity_rate) 59 | 60 | M = let 61 | P = 62 | randn(rng, num_primals, num_primals) .* 63 | rand(rng, bernoulli, num_primals, num_primals) 64 | P' * P 65 | end 66 | 67 | A = 68 | randn(rng, num_inequalities, num_primals) .* 69 | rand(rng, bernoulli, num_inequalities, num_primals) 70 | b = randn(rng, num_inequalities) 71 | ϕ = randn(rng, num_primals) 72 | 73 | [reshape(M, length(M)); reshape(A, length(A)); b; ϕ] 74 | end 75 | 76 | "Unpack a parameter vector θ into the components of a convex QP." 77 | function unpack_parameters(::QuadraticProgramBenchmark, θ; num_primals, num_inequalities) 78 | M = reshape(θ[1:(num_primals^2)], num_primals, num_primals) 79 | A = reshape( 80 | θ[(num_primals^2 + 1):(num_primals^2 + num_inequalities * num_primals)], 81 | num_inequalities, 82 | num_primals, 83 | ) 84 | 85 | b = 86 | θ[(num_primals^2 + num_inequalities * num_primals + 1):(num_primals^2 + num_inequalities * (num_primals + 1))] 87 | ϕ = θ[(num_primals^2 + num_inequalities * (num_primals + 1) + 1):end] 88 | 89 | (; M, A, b, ϕ) 90 | end 91 | -------------------------------------------------------------------------------- /examples/lane_change.jl: -------------------------------------------------------------------------------- 1 | "Utility to create the road environment." 2 | function setup_road_environment(; lane_width = 2, num_lanes = 2, height = 50) 3 | lane_centers = map(lane_idx -> (lane_idx - 0.5) * lane_width, 1:num_lanes) 4 | vertices = [ 5 | [first(lane_centers) - 0.5lane_width, 0], 6 | [last(lane_centers) + 0.5lane_width, 0], 7 | [last(lane_centers) + 0.5lane_width, height], 8 | [first(lane_centers) - 0.5lane_width, height], 9 | ] 10 | 11 | (; lane_centers, environment = PolygonEnvironment(vertices)) 12 | end 13 | 14 | "Utility to set up a (two player) trajectory game." 15 | function setup_trajectory_game(; environment) 16 | cost = let 17 | stage_costs = map(1:2) do ii 18 | (x, u, t, θi) -> let 19 | lane_preference = last(θi) 20 | 21 | (x[Block(ii)][1] - lane_preference)^2 + 22 | 0.5norm_sqr(x[Block(ii)][3:4] - [0, 2]) + 23 | 0.1norm_sqr(u[Block(ii)]) 24 | end 25 | end 26 | 27 | function reducer(stage_costs) 28 | reduce(+, stage_costs) / length(stage_costs) 29 | end 30 | 31 | TimeSeparableTrajectoryGameCost( 32 | stage_costs, 33 | reducer, 34 | GeneralSumCostStructure(), 35 | 1.0, 36 | ) 37 | end 38 | 39 | function coupling_constraints(xs, us, θ) 40 | mapreduce(vcat, xs) do x 41 | x1, x2 = blocks(x) 42 | 43 | # Players need to stay at least 2 m away from one another. 44 | norm_sqr(x1[1:2] - x2[1:2]) - 4 45 | end 46 | end 47 | 48 | agent_dynamics = planar_double_integrator(; 49 | state_bounds = (; lb = [-Inf, -Inf, -10, 0], ub = [Inf, Inf, 10, 10]), 50 | control_bounds = (; lb = [-5, -5], ub = [3, 3]), 51 | ) 52 | dynamics = ProductDynamics([agent_dynamics for _ in 1:2]) 53 | 54 | TrajectoryGame(dynamics, cost, environment, coupling_constraints) 55 | end 56 | 57 | function run_lane_change_example(; 58 | initial_state = mortar([[1.0, 1.0, 0.0, 1.0], [3.2, 0.9, 0.0, 1.0]]), 59 | horizon = 10, 60 | height = 50.0, 61 | num_lanes = 2, 62 | lane_width = 2, 63 | num_sim_steps = 150, 64 | ) 65 | (; environment, lane_centers) = 66 | setup_road_environment(; num_lanes, lane_width, height) 67 | game = setup_trajectory_game(; environment) 68 | 69 | # Build a game. Each player has a parameter for lane preference. 70 | # P1 wants to stay in the left lane, and P2 wants to move from the 71 | # right to the left lane. 72 | lane_preferences = mortar([[lane_centers[1]], [lane_centers[1]]]) 73 | parametric_game = build_parametric_game(; game, horizon, params_per_player = 1) 74 | 75 | # Simulate the ground truth. 76 | turn_length = 3 77 | sim_steps = let 78 | progress = ProgressMeter.Progress(num_sim_steps) 79 | ground_truth_strategy = WarmStartRecedingHorizonStrategy(; 80 | game, 81 | parametric_game, 82 | turn_length, 83 | horizon, 84 | parameters = lane_preferences, 85 | ) 86 | 87 | rollout( 88 | game.dynamics, 89 | ground_truth_strategy, 90 | initial_state, 91 | num_sim_steps; 92 | get_info = (γ, x, t) -> 93 | (ProgressMeter.next!(progress); γ.receding_horizon_strategy), 94 | ) 95 | end 96 | 97 | animate_sim_steps( 98 | game, 99 | sim_steps; 100 | live = false, 101 | framerate = 20, 102 | show_turn = true, 103 | xlims = (first(lane_centers) - lane_width, last(lane_centers) + lane_width), 104 | ylims = (-1, height + 1), 105 | aspect = num_lanes * lane_width / height, 106 | ) 107 | end 108 | -------------------------------------------------------------------------------- /src/AutoDiff.jl: -------------------------------------------------------------------------------- 1 | """ Support for automatic differentiation of an MCP's solution (x, y) with respect 2 | to its parameters θ. Since a solution satisfies 3 | F(z; θ, ϵ) = 0 4 | for the primal-dual system, the derivative we are looking for is given by 5 | ∂z∂θ = -(∇F_z)⁺ ∇F_θ. 6 | 7 | Modifed from https://github.com/JuliaGameTheoreticPlanning/ParametricMCPs.jl/blob/main/src/AutoDiff.jl. 8 | """ 9 | 10 | module AutoDiff 11 | 12 | using ..MixedComplementarityProblems: MixedComplementarityProblems 13 | using ChainRulesCore: ChainRulesCore 14 | using ForwardDiff: ForwardDiff 15 | using LinearAlgebra: LinearAlgebra 16 | using SymbolicTracingUtils: SymbolicTracingUtils 17 | 18 | function _solve_jacobian_θ(mcp::MixedComplementarityProblems.PrimalDualMCP, solution, θ) 19 | !isnothing(mcp.∇F_θ!) || throw( 20 | ArgumentError( 21 | "Missing sensitivities. Set `compute_sensitivities = true` when constructing the PrimalDualMCP.", 22 | ), 23 | ) 24 | 25 | (; x, y, s, ϵ) = solution 26 | 27 | ∇F_z = let 28 | ∇F = mcp.∇F_z!.result_buffer 29 | mcp.∇F_z!(∇F, x, y, s; θ, ϵ) 30 | ∇F 31 | end 32 | 33 | ∇F_θ = let 34 | ∇F = mcp.∇F_θ!.result_buffer 35 | mcp.∇F_θ!(∇F, x, y, s; θ, ϵ) 36 | ∇F 37 | end 38 | 39 | LinearAlgebra.qr(-collect(∇F_z), LinearAlgebra.ColumnNorm()) \ collect(∇F_θ) 40 | end 41 | 42 | function ChainRulesCore.rrule( 43 | ::typeof(MixedComplementarityProblems.solve), 44 | solver_type::MixedComplementarityProblems.SolverType, 45 | mcp::MixedComplementarityProblems.PrimalDualMCP, 46 | θ; 47 | kwargs..., 48 | ) 49 | solution = MixedComplementarityProblems.solve(solver_type, mcp, θ; kwargs...) 50 | project_to_θ = ChainRulesCore.ProjectTo(θ) 51 | 52 | function solve_pullback(∂solution) 53 | no_grad_args = (; 54 | ∂self = ChainRulesCore.NoTangent(), 55 | ∂solver_type = ChainRulesCore.NoTangent(), 56 | ∂mcp = ChainRulesCore.NoTangent(), 57 | ) 58 | 59 | ∂θ = ChainRulesCore.@thunk let 60 | ∂z∂θ = _solve_jacobian_θ(mcp, solution, θ) 61 | ∂l∂x = ∂solution.x 62 | ∂l∂y = ∂solution.y 63 | ∂l∂s = ∂solution.s 64 | 65 | @views project_to_θ( 66 | ∂z∂θ[1:(mcp.unconstrained_dimension), :]' * ∂l∂x + 67 | ∂z∂θ[ 68 | (mcp.unconstrained_dimension + 1):(mcp.unconstrained_dimension + mcp.constrained_dimension), 69 | :, 70 | ]' * ∂l∂y + 71 | ∂z∂θ[ 72 | (mcp.unconstrained_dimension + mcp.constrained_dimension + 1):end, 73 | :, 74 | ]' * ∂l∂s, 75 | ) 76 | end 77 | 78 | no_grad_args..., ∂θ 79 | end 80 | 81 | solution, solve_pullback 82 | end 83 | 84 | function MixedComplementarityProblems.solve( 85 | solver_type::MixedComplementarityProblems.InteriorPoint, 86 | mcp::MixedComplementarityProblems.PrimalDualMCP, 87 | θ::AbstractVector{<:ForwardDiff.Dual{T}}; 88 | kwargs..., 89 | ) where {T} 90 | # strip off the duals 91 | θ_v = ForwardDiff.value.(θ) 92 | θ_p = ForwardDiff.partials.(θ) 93 | # forward pass 94 | solution = MixedComplementarityProblems.solve(solver_type, mcp, θ_v; kwargs...) 95 | # backward pass 96 | ∂z∂θ = _solve_jacobian_θ(mcp, solution, θ_v) 97 | # downstream gradient 98 | z_p = ∂z∂θ * θ_p 99 | # glue forward and backward pass together into dual number types 100 | x_d = ForwardDiff.Dual{T}.(solution.x, @view z_p[1:(mcp.unconstrained_dimension)]) 101 | y_d = 102 | ForwardDiff.Dual{ 103 | T, 104 | }.( 105 | solution.y, 106 | @view z_p[(mcp.unconstrained_dimension + 1):(mcp.unconstrained_dimension + mcp.constrained_dimension)] 107 | ) 108 | s_d = 109 | ForwardDiff.Dual{ 110 | T, 111 | }.( 112 | solution.y, 113 | @view z_p[(mcp.unconstrained_dimension + mcp.constrained_dimension + 1):end] 114 | ) 115 | 116 | (; solution.status, solution.kkt_error, solution.ϵ, x = x_d, y = y_d, s = s_d) 117 | end 118 | 119 | end 120 | -------------------------------------------------------------------------------- /test/runtests.jl: -------------------------------------------------------------------------------- 1 | using Test: @testset, @test 2 | 3 | using MixedComplementarityProblems 4 | using BlockArrays: BlockArray, Block, mortar, blocks 5 | using Zygote: Zygote 6 | using FiniteDiff: FiniteDiff 7 | 8 | @testset "QPTestProblem" begin 9 | """ Test for the following QP: 10 | min_x 0.5 xᵀ M x - θᵀ x 11 | s.t. Ax - b ≥ 0. 12 | Taking `y ≥ 0` as a Lagrange multiplier, we obtain the KKT conditions: 13 | G(x, y) = Mx - Aᵀy - θ = 0 14 | 0 ≤ y ⟂ H(x, y) = Ax - b ≥ 0. 15 | """ 16 | M = [2 1; 1 2] 17 | A = [1 0; 0 1] 18 | b = [1; 1] 19 | θ = [-0.5; 0.5] 20 | 21 | G(x, y; θ) = M * x - θ - A' * y 22 | H(x, y; θ) = A * x - b 23 | K(z; θ) = begin 24 | x = z[1:size(M, 1)] 25 | y = z[(size(M, 1) + 1):end] 26 | 27 | [G(x, y; θ); H(x, y; θ)] 28 | end 29 | 30 | function check_solution(sol) 31 | @test all(abs.(G(sol.x, sol.y; θ)) .≤ 5e-3) 32 | @test all(H(sol.x, sol.y; θ) .≥ 0) 33 | @test all(sol.y .≥ 0) 34 | @test sum(sol.y .* H(sol.x, sol.y; θ)) ≤ 5e-3 35 | @test all(sol.s .≤ 5e-3) 36 | @test sol.kkt_error ≤ 5e-3 37 | @test sol.status == :solved 38 | end 39 | 40 | @testset "BasicCallableConstructor" begin 41 | mcp = MixedComplementarityProblems.PrimalDualMCP( 42 | G, 43 | H; 44 | unconstrained_dimension = size(M, 1), 45 | constrained_dimension = length(b), 46 | parameter_dimension = size(M, 1), 47 | ) 48 | sol = MixedComplementarityProblems.solve(MixedComplementarityProblems.InteriorPoint(), mcp, θ) 49 | 50 | check_solution(sol) 51 | end 52 | 53 | @testset "AlternativeCallableConstructor" begin 54 | mcp = MixedComplementarityProblems.PrimalDualMCP( 55 | K, 56 | [fill(-Inf, size(M, 1)); fill(0, length(b))], 57 | fill(Inf, size(M, 1) + length(b)); 58 | parameter_dimension = size(M, 1), 59 | ) 60 | sol = MixedComplementarityProblems.solve(MixedComplementarityProblems.InteriorPoint(), mcp, θ) 61 | 62 | check_solution(sol) 63 | end 64 | 65 | @testset "AutodifferentationTests" begin 66 | mcp = MixedComplementarityProblems.PrimalDualMCP( 67 | G, 68 | H; 69 | unconstrained_dimension = size(M, 1), 70 | constrained_dimension = length(b), 71 | parameter_dimension = size(M, 1), 72 | compute_sensitivities = true, 73 | ) 74 | 75 | function f(θ) 76 | sol = MixedComplementarityProblems.solve(MixedComplementarityProblems.InteriorPoint(), mcp, θ) 77 | sum(sol.x .^ 2) + sum(sol.y .^ 2) 78 | end 79 | 80 | ∇_autodiff_reverse = only(Zygote.gradient(f, θ)) 81 | ∇_autodiff_forward = only(Zygote.gradient(θ -> Zygote.forwarddiff(f, θ), θ)) 82 | ∇_finitediff = FiniteDiff.finite_difference_gradient(f, θ) 83 | @test isapprox(∇_autodiff_reverse, ∇_finitediff; atol = 1e-3) 84 | @test isapprox(∇_autodiff_reverse, ∇_autodiff_forward; atol = 1e-3) 85 | end 86 | end 87 | 88 | @testset "ParametricGameTests" begin 89 | """ Test the game -> MCP interface. """ 90 | lim = 0.5 91 | game = MixedComplementarityProblems.ParametricGame(; 92 | test_point = mortar([[1, 1], [1, 1]]), 93 | test_parameter = mortar([[1, 1], [1, 1]]), 94 | problems = [ 95 | MixedComplementarityProblems.OptimizationProblem(; 96 | objective = (x, θi) -> sum((x[Block(1)] - θi) .^ 2), 97 | private_inequality = (x, θi) -> 98 | [-x[Block(1)] .+ lim; x[Block(1)] .+ lim], 99 | ), 100 | MixedComplementarityProblems.OptimizationProblem(; 101 | objective = (x, θi) -> sum((x[Block(2)] - θi) .^ 2), 102 | private_inequality = (x, θi) -> 103 | [-x[Block(2)] .+ lim; x[Block(2)] .+ lim], 104 | ), 105 | ], 106 | ) 107 | 108 | θ = mortar([[-1, 0], [1, 1]]) 109 | tol = 1e-4 110 | (; status, primals, variables, kkt_error) = MixedComplementarityProblems.solve(game, θ; tol) 111 | 112 | for ii in 1:2 113 | @test all(isapprox.(primals[ii], clamp.(θ[Block(ii)], -lim, lim), atol = 10tol)) 114 | end 115 | @test status == :solved 116 | end 117 | -------------------------------------------------------------------------------- /benchmark/path.jl: -------------------------------------------------------------------------------- 1 | "Benchmark interior point solver against PATH on a bunch of random test problems." 2 | function benchmark( 3 | benchmark_type; 4 | num_samples = 100, 5 | problem_kwargs = (;), 6 | ip_mcp = nothing, 7 | path_mcp = nothing, 8 | ip_kwargs = (; tol = 1e-6), 9 | ) 10 | # Generate problem and random parameters. 11 | @info "Generating random problems..." 12 | problem = generate_test_problem(benchmark_type; problem_kwargs...) 13 | 14 | rng = Random.MersenneTwister(1) 15 | θs = map(1:num_samples) do _ 16 | generate_random_parameter(benchmark_type; rng, problem_kwargs...) 17 | end 18 | 19 | # Generate corresponding MCPs. 20 | @info "Generating IP MCP..." 21 | parameter_dimension = length(first(θs)) 22 | ip_mcp = if !isnothing(ip_mcp) 23 | ip_mcp 24 | elseif hasproperty(problem, :K) 25 | # Generated a callable problem. 26 | MixedComplementarityProblems.PrimalDualMCP( 27 | problem.K, 28 | problem.lower_bounds, 29 | problem.upper_bounds; 30 | parameter_dimension, 31 | ) 32 | else 33 | # Generated a symbolic problem. 34 | MixedComplementarityProblems.PrimalDualMCP( 35 | problem.K_symbolic, 36 | problem.z_symbolic, 37 | problem.θ_symbolic, 38 | problem.lower_bounds, 39 | problem.upper_bounds; 40 | η_symbolic = hasproperty(problem, :η_symbolic) ? problem.η_symbolic : nothing, 41 | ) 42 | end 43 | 44 | @info "Generating PATH MCP..." 45 | path_mcp = if !isnothing(path_mcp) 46 | path_mcp 47 | elseif hasproperty(problem, :K) 48 | # Generated a callable problem. 49 | ParametricMCPs.ParametricMCP( 50 | (z, θ) -> problem.K(z; θ), 51 | problem.lower_bounds, 52 | problem.upper_bounds, 53 | parameter_dimension, 54 | ) 55 | else 56 | # Generated a symbolic problem. 57 | K_symbolic = 58 | !hasproperty(problem, :η_symbolic) ? problem.K_symbolic : 59 | Vector{Symbolics.Num}( 60 | Symbolics.substitute( 61 | problem.K_symbolic, 62 | Dict([problem.η_symbolic => 0.0]), 63 | ), 64 | ) 65 | 66 | ParametricMCPs.ParametricMCP( 67 | K_symbolic, 68 | problem.z_symbolic, 69 | problem.θ_symbolic, 70 | problem.lower_bounds, 71 | problem.upper_bounds; 72 | ) 73 | end 74 | 75 | # Warm up the solvers. 76 | @info "Warming up IP solver..." 77 | MixedComplementarityProblems.solve( 78 | MixedComplementarityProblems.InteriorPoint(), 79 | ip_mcp, 80 | first(θs); 81 | ip_kwargs..., 82 | ) 83 | 84 | @info "Warming up PATH solver..." 85 | ParametricMCPs.solve(path_mcp, first(θs); warn_on_convergence_failure = false) 86 | 87 | # Solve and time. 88 | ip_data = @showprogress desc = "Solving IP MCPs..." map(θs) do θ 89 | elapsed_time = @elapsed sol = MixedComplementarityProblems.solve( 90 | MixedComplementarityProblems.InteriorPoint(), 91 | ip_mcp, 92 | θ; 93 | ip_kwargs..., 94 | ) 95 | 96 | (; elapsed_time, success = sol.status == :solved) 97 | end 98 | 99 | path_data = @showprogress desc = "Solving PATH MCPs..." map(θs) do θ 100 | # Solve and time. 101 | elapsed_time = @elapsed sol = 102 | ParametricMCPs.solve(path_mcp, θ; warn_on_convergence_failure = false) 103 | 104 | (; elapsed_time, success = sol.status == PATHSolver.MCP_Solved) 105 | end 106 | 107 | (; ip_mcp, path_mcp, ip_data, path_data) 108 | end 109 | 110 | "Compute summary statistics from solver benchmark data." 111 | function summary_statistics(data) 112 | accumulate_stats(solver_data) = begin 113 | (; success_rate = fraction_solved(solver_data), runtime_stats(solver_data)...) 114 | end 115 | 116 | stats = 117 | (; ip = accumulate_stats(data.ip_data), path = accumulate_stats(data.path_data)) 118 | @info "IP runtime is $(100(stats.ip.μ / stats.path.μ)) % that of PATH." 119 | 120 | stats 121 | end 122 | 123 | "Estimate mean and standard deviation of runtimes for all problems." 124 | function runtime_stats(solver_data) 125 | filtered_times = 126 | map(datum -> datum.elapsed_time, filter(datum -> datum.success, solver_data)) 127 | μ = Statistics.mean(filtered_times) 128 | σ = Statistics.stdm(filtered_times, μ) 129 | 130 | (; μ, σ) 131 | end 132 | 133 | "Compute fraction of problems solved." 134 | function fraction_solved(solver_data) 135 | Statistics.mean(datum -> datum.success, solver_data) 136 | end 137 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MixedComplementarityProblems.jl 2 | 3 | [![CI](https://github.com/CLeARoboticsLab/MixedComplementarityProblems.jl/actions/workflows/test.yml/badge.svg)](https://github.com/CLeARoboticsLab/MixedComplementarityProblems.jl/actions/workflows/test.yml) 4 | [![License](https://img.shields.io/badge/license-BSD-new)](https://opensource.org/license/bsd-3-clause) 5 | 6 | This package provides an easily-customizable interface for expressing mixed complementarity problems (MCPs) which are defined in terms of an arbitrary vector of parameters. `MixedComplementarityProblems` implements a reasonably high-performance interior point method for solving these problems, and integrates with `ChainRulesCore` and `ForwardDiff` to enable automatic differentiation of solutions with respect to problem parameters. 7 | 8 | ## What are MCPs? 9 | 10 | Mixed complementarity problems (MCPs) are a class of mathematical program, and they arise in a wide variety of application problems. In particular, one way they can arise is via the KKT conditions of nonlinear programs and noncooperative games. This package provides a utility for constructing MCPs from (parameterized) games, cf. `src/game.jl` for further details. To see the connection between KKT conditions and MCPs, read the next section. 11 | 12 | ## Why this package? 13 | 14 | As discussed below, this package replicates functionality already available in [ParametricMCPs](https://github.com/JuliaGameTheoreticPlanning/ParametricMCPs.jl). Our intention here is to provide an easily customizable and open-source solver with efficiency and reliability that is at least comparable with the [PATH](https://pages.cs.wisc.edu/~ferris/path.html) solver which `ParametricMCPs` uses under the hood (actually, it hooks into the interface to the `PATH` binaries which is provided by another wonderful package, [PATHSolver](https://github.com/chkwon/PATHSolver.jl)). Hopefully, users will find it useful to modify the interior point solver provided in this package for their own application problems, use it for highly parallelized implementations (since it is in pure Julia), etc. 15 | 16 | ## Installation 17 | 18 | `MixedComplementarityProblems` is a registered package and can be installed with the standard Julia package manager as follows: 19 | ```julia 20 | ] add MixedComplementarityProblems 21 | ``` 22 | 23 | ## Quickstart guide 24 | 25 | Suppose we have the following quadratic program: 26 | ```displaymath 27 | min_x 0.5 xᵀ M x - θᵀ x 28 | s.t. Ax - b ≥ 0. 29 | ``` 30 | 31 | The KKT conditions for this problem can be expressed as follows: 32 | ```displaymath 33 | G(x, y; θ) = Mx - θ - Aᵀ y = 0 34 | H(x, y; θ) = Ax - b ≥ 0 35 | y ≥ 0 36 | yᵀ H(x, y; θ) = 0, 37 | ``` 38 | where `y` is the Lagrange multiplier associated to the constraint `Ax - b ≥ 0` in the original problem. 39 | 40 | This is precisely a MCP, whose standard form is: 41 | ```displaymath 42 | G(x, y; θ) = 0 43 | 0 ≤ y ⟂ H(x, y; θ) ≥ 0. 44 | ``` 45 | 46 | Now, we can encode this problem and solve it using `MixedComplementarityProblems` as follows: 47 | 48 | ```julia 49 | using MixedComplementarityProblems 50 | 51 | M = [2 1; 1 2] 52 | A = [1 0; 0 1] 53 | b = [1; 1] 54 | θ = rand(2) 55 | 56 | G(x, y; θ) = M * x - θ - A' * y 57 | H(x, y; θ) = A * x - b 58 | 59 | mcp = MixedComplementarityProblems.PrimalDualMCP( 60 | G, 61 | H; 62 | unconstrained_dimension = size(M, 1), 63 | constrained_dimension = length(b), 64 | parameter_dimension = size(M, 1), 65 | ) 66 | sol = MixedComplementarityProblems.solve(MixedComplementarityProblems.InteriorPoint(), mcp, θ) 67 | ``` 68 | 69 | The solver can easily be warm-started from a given initial guess: 70 | ```julia 71 | sol = MixedComplementarityProblems.solve( 72 | MixedComplementarityProblems.InteriorPoint(), 73 | mcp, 74 | θ; 75 | x₀ = # your initial guess 76 | y₀ = # your **positive** initial guess 77 | ) 78 | ``` 79 | 80 | Note that the initial guess for the $y$ variable must be elementwise positive. This is because we are using an interior point method; for further details, refer to `src/solver.jl`. 81 | 82 | Finally, `MixedComplementarityProblems` integrates with `ChainRulesCore` and `ForwardDiff` so you can differentiate through the solver itself! For example, suppose we wanted to find the value of $\theta$ in the problem above which solves 83 | ```displaymath 84 | min_{θ, x, y} f(x, y) 85 | s.t. (x, y) solves MCP(θ). 86 | ``` 87 | 88 | We could do so by initializing with a particular value of $\theta$ and then iteratively descending the gradient $\nabla_\theta f$, which we can easily compute via: 89 | ```julia 90 | mcp = MixedComplementarityProblems.PrimalDualMCP( 91 | G, 92 | H; 93 | unconstrained_dimension = size(M, 1), 94 | constrained_dimension = length(b), 95 | parameter_dimension = size(M, 1), 96 | compute_sensitivities = true, 97 | ) 98 | 99 | function f(θ) 100 | sol = MixedComplementarityProblems.solve(MixedComplementarityProblems.InteriorPoint(), mcp, θ) 101 | 102 | # Some example objective function that depends on `x` and `y`. 103 | sum(sol.x .^ 2) + sum(sol.y .^ 2) 104 | end 105 | 106 | ∇f = only(Zygote.gradient(f, θ)) 107 | ``` 108 | 109 | ## A fancier demo 110 | 111 | If you'd like to get a better sense of the kinds of problems `MixedComplementarityProblems` was built for, check out the example in `examples/lane_change.jl`. This problem encodes a two-player game in which each player is driving a car and wishes to choose a trajectory that tracks a preferred lane center, maintains a desired speed, minimizes control actuation effort, and avoids collision with the other player. The problem is naturally expressed as a noncooperative game, and encoded as a mixed complementarity problem. 112 | 113 | To run the example, activate the `examples` environment 114 | ```julia 115 | ] activate examples 116 | ``` 117 | and then, from within the examples directory run 118 | ```julia 119 | include("TrajectoryExamples") 120 | TrajectoryExamples.run_lane_change_example() 121 | ``` 122 | 123 | This will generate a video animation and save it as `sim_steps.mp4`, and it should show two vehicles smoothly changing lanes and avoiding collisions. Once compiled, the entire example should run in a few seconds (including time to save everything). 124 | 125 | ## Acknowledgement and future plans 126 | 127 | This project inherits many key ideas from [ParametricMCPs](https://github.com/JuliaGameTheoreticPlanning/ParametricMCPs.jl), which provides essentially identical functionality but which currently only supports the (closed-source, but otherwise excellent) [PATH](https://pages.cs.wisc.edu/~ferris/path.html) solver. Ultimately, this `MixedComplementarityProblems` will likely merge with `ParametricMCPs` to provide an identical frontend and allow users a flexible choice of backend solver. Currently, `MixedComplementarityProblems` replicates a substantially similar interface as that provided by `ParametricMCPs`, but there are some (potentially annoying) differences that users should take care to notice, e.g., in the function signature for `solve(...)`. 128 | 129 | ## Other related projects 130 | 131 | If you are curious about other related software, consider checking out [JuliaGameTheoreticPlanning](https://github.com/orgs/JuliaGameTheoreticPlanning/repositories). 132 | -------------------------------------------------------------------------------- /src/solver.jl: -------------------------------------------------------------------------------- 1 | abstract type SolverType end 2 | struct InteriorPoint <: SolverType end 3 | 4 | """ Basic interior point solver, based on Nocedal & Wright, ch. 19. 5 | Computes step directions `δz` by solving the relaxed primal-dual system, i.e. 6 | ∇F(z; ϵ) δz = -F(z; ϵ). 7 | 8 | Given a step direction `δz`, performs a "fraction to the boundary" linesearch, 9 | i.e., for `(x, s)` it chooses step size `α_s` such that 10 | α_s = max(α ∈ [0, 1] : s + α δs ≥ (1 - τ) s) 11 | and for `y` it chooses step size `α_s` such that 12 | α_y = max(α ∈ [0, 1] : y + α δy ≥ (1 - τ) y). 13 | 14 | A typical value of τ is 0.995. Once we converge to ||F(z; \epsilon)|| ≤ ϵ, 15 | we typically decrease ϵ by a factor of 0.1 or 0.2, with smaller values chosen 16 | when the previous subproblem is solved in fewer iterations. 17 | 18 | Positional arguments: 19 | - `mcp::PrimalDualMCP`: the mixed complementarity problem to solve. 20 | - `θ::AbstractVector{<:Real}`: the parameter vector. 21 | 22 | Keyword arguments: 23 | - `x₀::AbstractVector{<:Real}`: the initial primal variable. 24 | - `y₀::AbstractVector{<:Real}`: the initial dual variable. 25 | - `s₀::AbstractVector{<:Real}`: the initial slack variable. 26 | - `ϵ₀::Real`: the initial relaxation scale. 27 | - `tol::Real = 1e-4`: the tolerance for the KKT error. 28 | - `max_inner_iters::Int = 20`: the maximum number of inner iterations. 29 | - `max_outer_iters::Int = 50`: the maximum number of outer iterations. 30 | - `tightening_rate::Real = 0.1`: rate for tightening tolerance and regularization. 31 | - `loosening_rate::Real = 0.5`: rate for loosening tolerance and regularization. 32 | - `min_stepsize::Real = 1e-2`: the minimum step size for the linesearch. 33 | - `verbose::Bool = false`: whether to print debug information. 34 | - `linear_solve_algorithm::LinearSolve.SciMLLinearSolveAlgorithm`: the linear solve algorithm to use. Any solver from `LinearSolve.jl` can be used. 35 | - `regularize_linear_solve::Symbol = :none`: scheme for regularizing the linear system matrix ∇F. Options are {:none, :identity, :internal}. 36 | """ 37 | function solve( 38 | ::InteriorPoint, 39 | mcp::PrimalDualMCP, 40 | θ::AbstractVector{<:Real}; 41 | x₀ = nothing, 42 | y₀ = nothing, 43 | s₀ = nothing, 44 | tol = 1e-4, 45 | ϵ₀ = :auto, 46 | max_inner_iters = 20, 47 | max_outer_iters = 50, 48 | tightening_rate = 0.1, 49 | loosening_rate = 0.5, 50 | min_stepsize = 1e-4, 51 | verbose = false, 52 | linear_solve_algorithm = UMFPACKFactorization(), 53 | regularize_linear_solve = :identity, 54 | ) 55 | # Set up common memory. 56 | ∇F = mcp.∇F_z!.result_buffer 57 | F = zeros(mcp.unconstrained_dimension + 2mcp.constrained_dimension) 58 | δz = zeros(mcp.unconstrained_dimension + 2mcp.constrained_dimension) 59 | δx = @view δz[1:(mcp.unconstrained_dimension)] 60 | δy = 61 | @view δz[(mcp.unconstrained_dimension + 1):(mcp.unconstrained_dimension + mcp.constrained_dimension)] 62 | δs = @view δz[(mcp.unconstrained_dimension + mcp.constrained_dimension + 1):end] 63 | 64 | linsolve = init(LinearProblem(∇F, δz), linear_solve_algorithm) 65 | 66 | # Initialize primal, dual, and slack variables. 67 | x = @something(x₀, zeros(mcp.unconstrained_dimension)) 68 | y = @something(y₀, ones(mcp.constrained_dimension)) 69 | s = @something(s₀, ones(mcp.constrained_dimension)) 70 | 71 | # Initialize IP relaxation parameter. 72 | if ϵ₀ === :auto 73 | is_warmstarted = !isnothing(x₀) && !isnothing(y₀) && !isnothing(s₀) 74 | if is_warmstarted 75 | ϵ = tol 76 | else 77 | ϵ = one(tol) 78 | end 79 | else 80 | ϵ = ϵ₀ 81 | end 82 | 83 | # Initialize regularization parameter. 84 | η = tol 85 | 86 | # Main solver loop. 87 | status = :solved 88 | total_iters = 0 89 | inner_iters = 1 90 | outer_iters = 1 91 | kkt_error = Inf 92 | while outer_iters < max_outer_iters || iszero(total_iters) 93 | inner_iters = 1 94 | status = :solved 95 | 96 | while kkt_error > ϵ && inner_iters < max_inner_iters 97 | total_iters += 1 98 | 99 | # Compute the (regularized) Newton step. 100 | # TODO: use a linear operator with a lazy gradient computation here. 101 | if regularize_linear_solve === :internal 102 | mcp.F!(F, x, y, s; θ, ϵ, η = 0.0) 103 | mcp.∇F_z!(∇F, x, y, s; θ, ϵ, η) 104 | else 105 | mcp.F!(F, x, y, s; θ, ϵ) 106 | mcp.∇F_z!(∇F, x, y, s; θ, ϵ) 107 | end 108 | 109 | if regularize_linear_solve === :identity 110 | if size(∇F, 1) == size(∇F, 2) 111 | linsolve.A = ∇F + η * I 112 | else 113 | @warn "Cannot use identity regularization on a nonsquare problem." 114 | end 115 | else 116 | linsolve.A = ∇F 117 | end 118 | 119 | linsolve.b = -F 120 | solution = solve!(linsolve) 121 | 122 | if !SciMLBase.successful_retcode(solution) && 123 | (solution.retcode !== SciMLBase.ReturnCode.Default) 124 | verbose && 125 | @warn "Linear solve failed. Exiting prematurely. Return code: $(solution.retcode)" 126 | status = :failed 127 | break 128 | end 129 | 130 | δz .= solution.u 131 | 132 | # Fraction to the boundary linesearch. 133 | α_s = fraction_to_the_boundary_linesearch(s, δs; tol = min_stepsize) 134 | α_y = fraction_to_the_boundary_linesearch(y, δy; tol = min_stepsize) 135 | 136 | if isnan(α_s) || isnan(α_y) 137 | verbose && @warn "Linesearch failed. Exiting prematurely." 138 | status = :failed 139 | break 140 | end 141 | 142 | # Update variables accordingly. 143 | @. x += α_s * δx 144 | @. s += α_s * δs 145 | @. y += α_y * δy 146 | 147 | kkt_error = norm(F, Inf) 148 | inner_iters += 1 149 | end 150 | 151 | if kkt_error <= ϵ <= tol 152 | break 153 | end 154 | 155 | if status === :solved 156 | ϵ *= 1 - exp(-tightening_rate * inner_iters) 157 | η *= 1 - exp(-tightening_rate * inner_iters) 158 | else 159 | ϵ *= 1 + exp(-loosening_rate * inner_iters) 160 | η *= 1 + exp(-loosening_rate * inner_iters) 161 | end 162 | ϵ = min(ϵ, one(ϵ)) 163 | outer_iters += 1 164 | end 165 | 166 | if outer_iters == max_outer_iters 167 | status = :failed 168 | end 169 | 170 | (; status, x, y, s, kkt_error, ϵ, outer_iters, total_iters) 171 | end 172 | 173 | """Helper function to compute the step size `α` which solves: 174 | α* = max(α ∈ [0, 1] : v + α δ ≥ (1 - τ) v). 175 | """ 176 | function fraction_to_the_boundary_linesearch(v, δ; τ = 0.995, decay = 0.5, tol = 1e-4) 177 | α = 1.0 178 | while any(@. v + α * δ < (1 - τ) * v) 179 | if α < tol 180 | return NaN 181 | end 182 | 183 | α *= decay 184 | end 185 | 186 | α 187 | end 188 | -------------------------------------------------------------------------------- /src/game.jl: -------------------------------------------------------------------------------- 1 | "Utility to represent a parameterized optimization problem." 2 | Base.@kwdef struct OptimizationProblem{T1,T2,T3} 3 | objective::T1 4 | private_equality::T2 = nothing 5 | private_inequality::T3 = nothing 6 | end 7 | 8 | """A structure to represent a game with objectives and constraints parameterized by 9 | a vector θ ∈ Rⁿ. 10 | 11 | We will assume that the players' primal variables are stacked into a BlockVector 12 | x with the i-th block corresponding to the i-th player's decision variable. 13 | Similarly, we will assume that the parameter θ has a block structure so that the 14 | i-th player's problem depends only upon the i-th block of θ. 15 | """ 16 | struct ParametricGame{T1,T2,T3} 17 | problems::Vector{<:OptimizationProblem} 18 | shared_equality::T1 19 | shared_inequality::T2 20 | dims::T3 21 | 22 | "PrimalDualMCP representation." 23 | mcp::PrimalDualMCP 24 | end 25 | 26 | function ParametricGame(; 27 | test_point, 28 | test_parameter, 29 | problems, 30 | shared_equality = nothing, 31 | shared_inequality = nothing, 32 | ) 33 | (; K_symbolic, z_symbolic, θ_symbolic, η_symbolic, lower_bounds, upper_bounds, dims) = 34 | game_to_mcp(; 35 | test_point, 36 | test_parameter, 37 | problems, 38 | shared_equality, 39 | shared_inequality, 40 | ) 41 | 42 | mcp = PrimalDualMCP( 43 | K_symbolic, 44 | z_symbolic, 45 | θ_symbolic, 46 | lower_bounds, 47 | upper_bounds; 48 | η_symbolic, 49 | ) 50 | 51 | ParametricGame(problems, shared_equality, shared_inequality, dims, mcp) 52 | end 53 | 54 | "Helper for converting game components to MCP components." 55 | function game_to_mcp(; 56 | test_point, 57 | test_parameter, 58 | problems, 59 | shared_equality = nothing, 60 | shared_inequality = nothing, 61 | ) 62 | N = length(problems) 63 | @assert N == length(blocks(test_point)) 64 | 65 | dims = dimensions( 66 | test_point, 67 | test_parameter, 68 | problems, 69 | shared_equality, 70 | shared_inequality, 71 | ) 72 | 73 | # Define primal and dual variables for the game, and game parameters. 74 | # Note that BlockArrays can handle blocks of zero size. 75 | backend = SymbolicTracingUtils.SymbolicsBackend() 76 | x = 77 | SymbolicTracingUtils.make_variables(backend, :x, sum(dims.x)) |> 78 | to_blockvector(dims.x) 79 | λ = 80 | SymbolicTracingUtils.make_variables(backend, :λ, sum(dims.λ)) |> 81 | to_blockvector(dims.λ) 82 | μ = 83 | SymbolicTracingUtils.make_variables(backend, :μ, sum(dims.μ)) |> 84 | to_blockvector(dims.μ) 85 | λ̃ = SymbolicTracingUtils.make_variables(backend, :λ̃, dims.λ̃) 86 | μ̃ = SymbolicTracingUtils.make_variables(backend, :μ̃, dims.μ̃) 87 | θ = 88 | SymbolicTracingUtils.make_variables(backend, :θ, sum(dims.θ)) |> 89 | to_blockvector(dims.θ) 90 | 91 | # Parameter for adding a scaled identity to the Hessian of each player's 92 | # Lagrangian wrt that player's variable. 93 | η = only(SymbolicTracingUtils.make_variables(backend, :η, 1)) 94 | 95 | # Build symbolic expressions for objectives and constraints. 96 | fs = map(problems, blocks(θ)) do p, θi 97 | p.objective(x, θi) 98 | end 99 | gs = map(problems, blocks(θ)) do p, θi 100 | isnothing(p.private_equality) ? nothing : p.private_equality(x, θi) 101 | end 102 | hs = map(problems, blocks(θ)) do p, θi 103 | isnothing(p.private_inequality) ? nothing : p.private_inequality(x, θi) 104 | end 105 | 106 | g̃ = isnothing(shared_equality) ? nothing : shared_equality(x, θ) 107 | h̃ = isnothing(shared_inequality) ? nothing : shared_inequality(x, θ) 108 | 109 | # Build gradient of each player's Lagrangian and include regularization. 110 | ∇Ls = map(fs, gs, hs, blocks(x), blocks(λ), blocks(μ)) do f, g, h, xi, λi, μi 111 | L = 112 | f - (isnothing(g) ? 0 : sum(λi .* g)) - (isnothing(h) ? 0 : sum(μi .* h)) - (isnothing(g̃) ? 0 : sum(λ̃ .* g̃)) - 113 | (isnothing(h̃) ? 0 : sum(μ̃ .* h̃)) 114 | SymbolicTracingUtils.gradient(L, xi) + η * xi 115 | end 116 | 117 | # Build MCP representation. 118 | symbolic_type = eltype(x) 119 | K = Vector{symbolic_type}( 120 | filter!( 121 | !isnothing, 122 | [ 123 | reduce(vcat, ∇Ls) 124 | reduce(vcat, gs) 125 | g̃ 126 | reduce(vcat, hs) 127 | h̃ 128 | ], 129 | ), 130 | ) 131 | 132 | z = Vector{symbolic_type}( 133 | filter!( 134 | !isnothing, 135 | [ 136 | x 137 | mapreduce(b -> length(b) == 0 ? nothing : b, vcat, blocks(λ)) 138 | length(λ̃) == 0 ? nothing : λ̃ 139 | mapreduce(b -> length(b) == 0 ? nothing : b, vcat, blocks(μ)) 140 | length(μ̃) == 0 ? nothing : μ̃ 141 | ], 142 | ), 143 | ) 144 | 145 | lower_bounds = [ 146 | fill(-Inf, length(x)) 147 | fill(-Inf, length(λ)) 148 | fill(-Inf, length(λ̃)) 149 | fill(0, length(μ)) 150 | fill(0, length(μ̃)) 151 | ] 152 | 153 | upper_bounds = [ 154 | fill(Inf, length(x)) 155 | fill(Inf, length(λ)) 156 | fill(Inf, length(λ̃)) 157 | fill(Inf, length(μ)) 158 | fill(Inf, length(μ̃)) 159 | ] 160 | 161 | (; 162 | K_symbolic = collect(K), 163 | z_symbolic = collect(z), 164 | θ_symbolic = collect(θ), 165 | η_symbolic = η, 166 | lower_bounds, 167 | upper_bounds, 168 | dims, 169 | ) 170 | end 171 | 172 | function dimensions( 173 | test_point, 174 | test_parameter, 175 | problems, 176 | shared_equality, 177 | shared_inequality, 178 | ) 179 | x = only(blocksizes(test_point)) 180 | θ = only(blocksizes(test_parameter)) 181 | λ = map(problems, blocks(test_parameter)) do p, θi 182 | isnothing(p.private_equality) ? 0 : length(p.private_equality(test_point, θi)) 183 | end 184 | μ = map(problems, blocks(test_parameter)) do p, θi 185 | isnothing(p.private_inequality) ? 0 : length(p.private_inequality(test_point, θi)) 186 | end 187 | 188 | λ̃ = 189 | isnothing(shared_equality) ? 0 : 190 | length(shared_equality(test_point, test_parameter)) 191 | μ̃ = 192 | isnothing(shared_inequality) ? 0 : 193 | length(shared_inequality(test_point, test_parameter)) 194 | 195 | (; x, θ, λ, μ, λ̃, μ̃) 196 | end 197 | 198 | "Solve a parametric game." 199 | function solve(game::ParametricGame, θ; solver_type = InteriorPoint(), kwargs...) 200 | (; x, y, s, kkt_error, status) = 201 | solve(solver_type, game.mcp, θ; regularize_linear_solve = :internal, kwargs...) 202 | 203 | # Unpack primals per-player for ease of access later. 204 | end_dims = cumsum(game.dims.x) 205 | primals = map(1:num_players(game)) do ii 206 | (ii == 1) ? x[1:end_dims[ii]] : x[(end_dims[ii - 1] + 1):end_dims[ii]] 207 | end 208 | 209 | (; primals, variables = (; x, y, s), kkt_error, status) 210 | end 211 | 212 | "Return the number of players in this game." 213 | function num_players(game::ParametricGame) 214 | length(game.problems) 215 | end 216 | -------------------------------------------------------------------------------- /src/mcp.jl: -------------------------------------------------------------------------------- 1 | """ Store key elements of the primal-dual KKT system for a MCP composed of 2 | functions G(.) and H(.) such that 3 | 0 = G(x, y; θ) 4 | 0 ≤ H(x, y; θ) ⟂ y ≥ 0. 5 | 6 | The primal-dual system arises when we introduce slack variable `s` and set 7 | G(x, y; θ) = 0 8 | H(x, y; θ) - s = 0 9 | s ⦿ y - ϵ = 0 10 | for some ϵ > 0. Define the function `F(x, y, s; θ, ϵ, [η])` to return the left 11 | hand side of this system of equations. Here, `η` is an optional nonnegative 12 | regularization parameter defined by "internally-regularized" problems. 13 | """ 14 | struct PrimalDualMCP{T1,T2,T3} 15 | "A callable `F!(result, x, y, s; θ, ϵ, [η])` to compute the KKT error in-place." 16 | F!::T1 17 | "A callable `∇F_z!(result, x, y, s; θ, ϵ, [η])` to compute ∇F wrt z in-place." 18 | ∇F_z!::T2 19 | "A callable `∇F_θ!(result, x, y, s; θ, ϵ, [η])` to compute ∇F wrt θ in-place." 20 | ∇F_θ!::T3 21 | "Dimension of unconstrained variable." 22 | unconstrained_dimension::Int 23 | "Dimension of constrained variable." 24 | constrained_dimension::Int 25 | end 26 | 27 | "Helper to construct a PrimalDualMCP from callable functions `G(.)` and `H(.)`." 28 | function PrimalDualMCP( 29 | G, 30 | H; 31 | unconstrained_dimension, 32 | constrained_dimension, 33 | parameter_dimension, 34 | compute_sensitivities = false, 35 | backend = SymbolicTracingUtils.SymbolicsBackend(), 36 | backend_options = (;), 37 | ) 38 | x_symbolic = SymbolicTracingUtils.make_variables(backend, :x, unconstrained_dimension) 39 | y_symbolic = SymbolicTracingUtils.make_variables(backend, :y, constrained_dimension) 40 | θ_symbolic = SymbolicTracingUtils.make_variables(backend, :θ, parameter_dimension) 41 | G_symbolic = G(x_symbolic, y_symbolic; θ = θ_symbolic) 42 | H_symbolic = H(x_symbolic, y_symbolic; θ = θ_symbolic) 43 | 44 | PrimalDualMCP( 45 | G_symbolic, 46 | H_symbolic, 47 | x_symbolic, 48 | y_symbolic, 49 | θ_symbolic; 50 | compute_sensitivities, 51 | backend_options, 52 | ) 53 | end 54 | 55 | "Construct a PrimalDualMCP from symbolic expressions of G(.) and H(.)." 56 | function PrimalDualMCP( 57 | G_symbolic::Vector{T}, 58 | H_symbolic::Vector{T}, 59 | x_symbolic::Vector{T}, 60 | y_symbolic::Vector{T}, 61 | θ_symbolic::Vector{T}, 62 | η_symbolic::Union{Nothing,T} = nothing; 63 | compute_sensitivities = false, 64 | backend_options = (;), 65 | ) where {T<:Union{SymbolicTracingUtils.FD.Node,SymbolicTracingUtils.Symbolics.Num}} 66 | # Create symbolic slack variable `s` and parameter `ϵ`. 67 | if T == SymbolicTracingUtils.FD.Node 68 | backend = SymbolicTracingUtils.FastDifferentiationBackend() 69 | else 70 | @assert T === SymbolicTracingUtils.Symbolics.Num 71 | backend = SymbolicTracingUtils.SymbolicsBackend() 72 | end 73 | 74 | s_symbolic = SymbolicTracingUtils.make_variables(backend, :s, length(y_symbolic)) 75 | ϵ_symbolic = only(SymbolicTracingUtils.make_variables(backend, :ϵ, 1)) 76 | z_symbolic = [x_symbolic; y_symbolic; s_symbolic] 77 | 78 | F_symbolic = [ 79 | G_symbolic 80 | H_symbolic - s_symbolic 81 | s_symbolic .* y_symbolic .- ϵ_symbolic 82 | ] 83 | 84 | F! = if isnothing(η_symbolic) 85 | _F! = SymbolicTracingUtils.build_function( 86 | F_symbolic, 87 | x_symbolic, 88 | y_symbolic, 89 | s_symbolic, 90 | θ_symbolic, 91 | ϵ_symbolic; 92 | in_place = true, 93 | backend_options, 94 | ) 95 | 96 | (result, x, y, s; θ, ϵ) -> _F!(result, x, y, s, θ, ϵ) 97 | else 98 | _F! = SymbolicTracingUtils.build_function( 99 | F_symbolic, 100 | x_symbolic, 101 | y_symbolic, 102 | s_symbolic, 103 | θ_symbolic, 104 | ϵ_symbolic, 105 | η_symbolic; 106 | in_place = true, 107 | backend_options, 108 | ) 109 | 110 | (result, x, y, s; θ, ϵ, η = 0.0) -> _F!(result, x, y, s, θ, ϵ, η) 111 | end 112 | 113 | function process_∇F(F, var) 114 | ∇F_symbolic = SymbolicTracingUtils.sparse_jacobian(F, var) 115 | rows, cols, _ = SparseArrays.findnz(∇F_symbolic) 116 | constant_entries = SymbolicTracingUtils.get_constant_entries(∇F_symbolic, var) 117 | 118 | if isnothing(η_symbolic) 119 | _∇F! = SymbolicTracingUtils.build_function( 120 | ∇F_symbolic, 121 | x_symbolic, 122 | y_symbolic, 123 | s_symbolic, 124 | θ_symbolic, 125 | ϵ_symbolic; 126 | in_place = true, 127 | backend_options, 128 | ) 129 | 130 | return SymbolicTracingUtils.SparseFunction( 131 | (result, x, y, s; θ, ϵ) -> _∇F!(result, x, y, s, θ, ϵ), 132 | rows, 133 | cols, 134 | size(∇F_symbolic), 135 | constant_entries, 136 | ) 137 | else 138 | _∇F! = SymbolicTracingUtils.build_function( 139 | ∇F_symbolic, 140 | x_symbolic, 141 | y_symbolic, 142 | s_symbolic, 143 | θ_symbolic, 144 | ϵ_symbolic, 145 | η_symbolic; 146 | in_place = true, 147 | backend_options, 148 | ) 149 | 150 | return SymbolicTracingUtils.SparseFunction( 151 | (result, x, y, s; θ, ϵ, η = 0.0) -> _∇F!(result, x, y, s, θ, ϵ, η), 152 | rows, 153 | cols, 154 | size(∇F_symbolic), 155 | constant_entries, 156 | ) 157 | end 158 | end 159 | 160 | ∇F_z! = process_∇F(F_symbolic, z_symbolic) 161 | ∇F_θ! = !compute_sensitivities ? nothing : process_∇F(F_symbolic, θ_symbolic) 162 | 163 | PrimalDualMCP(F!, ∇F_z!, ∇F_θ!, length(x_symbolic), length(y_symbolic)) 164 | end 165 | 166 | """ Construct a PrimalDualMCP from `K(z; θ) ⟂ z̲ ≤ z ≤ z̅`, where `K` is callable. 167 | NOTE: Assumes that all upper bounds are Inf, and lower bounds are either -Inf or 0. 168 | """ 169 | function PrimalDualMCP( 170 | K, 171 | lower_bounds::Vector, 172 | upper_bounds::Vector; 173 | parameter_dimension, 174 | internally_regularized = false, 175 | compute_sensitivities = false, 176 | backend = SymbolicTracingUtils.SymbolicsBackend(), 177 | backend_options = (;), 178 | ) 179 | z_symbolic = SymbolicTracingUtils.make_variables(backend, :z, length(lower_bounds)) 180 | θ_symbolic = SymbolicTracingUtils.make_variables(backend, :θ, parameter_dimension) 181 | K_symbolic = K(z_symbolic; θ = θ_symbolic) 182 | 183 | if internally_regularized 184 | η_symbolic = only(SymbolicTracingUtils.make_variables(backend, :η, 1)) 185 | 186 | return PrimalDualMCP( 187 | K_symbolic, 188 | z_symbolic, 189 | θ_symbolic, 190 | lower_bounds, 191 | upper_bounds; 192 | η_symbolic, 193 | compute_sensitivities, 194 | backend_options, 195 | ) 196 | end 197 | 198 | PrimalDualMCP( 199 | K_symbolic, 200 | z_symbolic, 201 | θ_symbolic, 202 | lower_bounds, 203 | upper_bounds; 204 | compute_sensitivities, 205 | backend_options, 206 | ) 207 | end 208 | 209 | """Construct a PrimalDualMCP from symbolic `K(z; θ) ⟂ z̲ ≤ z ≤ z̅`. 210 | NOTE: Assumes that all upper bounds are Inf, and lower bounds are either -Inf or 0. 211 | """ 212 | function PrimalDualMCP( 213 | K_symbolic::Vector{T}, 214 | z_symbolic::Vector{T}, 215 | θ_symbolic::Vector{T}, 216 | lower_bounds::Vector, 217 | upper_bounds::Vector; 218 | η_symbolic::Union{Nothing,T} = nothing, 219 | compute_sensitivities = false, 220 | backend_options = (;), 221 | ) where {T<:Union{SymbolicTracingUtils.FD.Node,SymbolicTracingUtils.Symbolics.Num}} 222 | @assert all(isinf.(upper_bounds)) && all(isinf.(lower_bounds) .|| lower_bounds .== 0) 223 | 224 | unconstrained_indices = findall(isinf, lower_bounds) 225 | constrained_indices = findall(!isinf, lower_bounds) 226 | 227 | G_symbolic = K_symbolic[unconstrained_indices] 228 | H_symbolic = K_symbolic[constrained_indices] 229 | x_symbolic = z_symbolic[unconstrained_indices] 230 | y_symbolic = z_symbolic[constrained_indices] 231 | 232 | PrimalDualMCP( 233 | G_symbolic, 234 | H_symbolic, 235 | x_symbolic, 236 | y_symbolic, 237 | θ_symbolic, 238 | η_symbolic; 239 | compute_sensitivities, 240 | backend_options, 241 | ) 242 | end 243 | -------------------------------------------------------------------------------- /examples/utils.jl: -------------------------------------------------------------------------------- 1 | "Utility for unpacking trajectory." 2 | function unpack_trajectory(flat_trajectories; dynamics::ProductDynamics) 3 | horizon = Int( 4 | length(flat_trajectories[Block(1)]) / 5 | (state_dim(dynamics, 1) + control_dim(dynamics, 1)), 6 | ) 7 | trajs = map(1:num_players(dynamics), blocks(flat_trajectories)) do ii, traj 8 | num_states = state_dim(dynamics, ii) * horizon 9 | X = reshape(traj[1:num_states], (state_dim(dynamics, ii), horizon)) 10 | U = reshape(traj[(num_states + 1):end], (control_dim(dynamics, ii), horizon)) 11 | 12 | (; xs = eachcol(X) |> collect, us = eachcol(U) |> collect) 13 | end 14 | 15 | stack_trajectories(trajs) 16 | end 17 | 18 | "Utility for packing trajectory." 19 | function pack_trajectory(traj) 20 | trajs = unstack_trajectory(traj) 21 | mapreduce(vcat, trajs) do τ 22 | [reduce(vcat, τ.xs); reduce(vcat, τ.us)] 23 | end 24 | end 25 | 26 | "Pack an initial state and set of other params into a single parameter vector." 27 | function pack_parameters(initial_state, other_params_per_player) 28 | mortar(map((x, θ) -> [x; θ], blocks(initial_state), blocks(other_params_per_player))) 29 | end 30 | 31 | "Unpack parameters into initial state and other parameters." 32 | function unpack_parameters(params; dynamics::ProductDynamics) 33 | initial_state = mortar(map(1:num_players(dynamics), blocks(params)) do ii, θi 34 | θi[1:state_dim(dynamics, ii)] 35 | end) 36 | other_params = mortar(map(1:num_players(dynamics), blocks(params)) do ii, θi 37 | θi[((state_dim(dynamics, ii) + 1):end)] 38 | end) 39 | 40 | (; initial_state, other_params) 41 | end 42 | 43 | "Generate a BitVector mask with 0 entries corresponding to the initial states." 44 | function parameter_mask(; dynamics::ProductDynamics, params_per_player) 45 | x = mortar([ones(state_dim(dynamics, i)) for i in 1:num_players(dynamics)]) 46 | x̃ = 2mortar([ones(state_dim(dynamics, i)) for i in 1:num_players(dynamics)]) 47 | θ = 3mortar([ones(kk) for kk in params_per_player]) 48 | 49 | pack_parameters(x, θ) .== pack_parameters(x̃, θ) 50 | end 51 | 52 | "Convert a TrajectoryGame to a PrimalDualMCP." 53 | function build_parametric_game(; 54 | game::TrajectoryGame, 55 | horizon = 10, 56 | params_per_player = 0, # not including initial state, which is always a param 57 | ) 58 | (; 59 | K_symbolic, 60 | z_symbolic, 61 | θ_symbolic, 62 | η_symbolic, 63 | lower_bounds, 64 | upper_bounds, 65 | dims, 66 | problems, 67 | shared_equality, 68 | shared_inequality, 69 | ) = build_mcp_components(; game, horizon, params_per_player) 70 | 71 | mcp = MixedComplementarityProblems.PrimalDualMCP( 72 | K_symbolic, 73 | z_symbolic, 74 | θ_symbolic, 75 | lower_bounds, 76 | upper_bounds; 77 | η_symbolic, 78 | ) 79 | MixedComplementarityProblems.ParametricGame( 80 | problems, 81 | shared_equality, 82 | shared_inequality, 83 | dims, 84 | mcp, 85 | ) 86 | end 87 | 88 | "Construct MCP components from game components." 89 | function build_mcp_components(; 90 | game::TrajectoryGame, 91 | horizon = 10, 92 | params_per_player = 0, # not including initial state, which is always a param 93 | ) 94 | N = 2 95 | N == num_players(game) || error("Should have only two players.") 96 | 97 | # Construct costs. 98 | function player_cost(τ, θi, ii) 99 | (; xs, us) = unpack_trajectory(τ; game.dynamics) 100 | ts = Iterators.eachindex(xs) 101 | map(xs, us, ts) do x, u, t 102 | game.cost.discount_factor^(t - 1) * game.cost.stage_cost[ii](x, u, t, θi) 103 | end |> game.cost.reducer 104 | end 105 | 106 | objectives = map(1:N) do ii 107 | (τ, θi) -> player_cost(τ, θi, ii) 108 | end 109 | 110 | # Shared equality constraints. 111 | shared_equality = (τ, θ) -> let 112 | (; xs, us) = unpack_trajectory(τ; game.dynamics) 113 | (; initial_state) = unpack_parameters(θ; game.dynamics) 114 | 115 | # Initial state constraint. 116 | g̃1 = xs[1] - initial_state 117 | 118 | # Dynamics constraints. 119 | ts = Iterators.eachindex(xs) 120 | g̃2 = mapreduce(vcat, ts[2:end]) do t 121 | xs[t] - game.dynamics(xs[t - 1], us[t - 1]) 122 | end 123 | 124 | vcat(g̃1, g̃2) 125 | end 126 | 127 | # Shared inequality constraints. 128 | shared_inequality = 129 | (τ, θ) -> let 130 | (; xs, us) = unpack_trajectory(τ; game.dynamics) 131 | 132 | # Collision-avoidance constriant. 133 | h̃1 = game.coupling_constraints(xs, us, θ) 134 | 135 | # Environment boundaries. 136 | env_constraints = TrajectoryGamesBase.get_constraints(game.env) 137 | h̃2 = mapreduce(vcat, xs) do x 138 | env_constraints(x) 139 | end 140 | 141 | # Actuator/state limits. 142 | actuator_constraint = TrajectoryGamesBase.get_constraints_from_box_bounds( 143 | control_bounds(game.dynamics), 144 | ) 145 | h̃3 = mapreduce(vcat, us) do u 146 | actuator_constraint(u) 147 | end 148 | 149 | state_constraint = TrajectoryGamesBase.get_constraints_from_box_bounds( 150 | state_bounds(game.dynamics), 151 | ) 152 | h̃4 = mapreduce(vcat, xs) do x 153 | state_constraint(x) 154 | end 155 | 156 | vcat(h̃1, h̃2, h̃3, h̃4) 157 | end 158 | 159 | primal_dims = [ 160 | horizon * (state_dim(game.dynamics, ii) + control_dim(game.dynamics, ii)) for 161 | ii in 1:N 162 | ] 163 | 164 | problems = map( 165 | f -> MixedComplementarityProblems.OptimizationProblem(; objective = f), 166 | objectives, 167 | ) 168 | 169 | components = MixedComplementarityProblems.game_to_mcp(; 170 | test_point = BlockArray(zeros(sum(primal_dims)), primal_dims), 171 | test_parameter = mortar([ 172 | zeros(state_dim(game.dynamics, ii) + params_per_player) for ii in 1:N 173 | ]), 174 | problems, 175 | shared_equality, 176 | shared_inequality, 177 | ) 178 | 179 | (; problems, shared_equality, shared_inequality, components...) 180 | end 181 | 182 | "Generate an initial guess for primal variables following a zero input sequence." 183 | function zero_input_trajectory(; 184 | game::TrajectoryGame{<:ProductDynamics}, 185 | horizon, 186 | initial_state, 187 | ) 188 | rollout_strategy = 189 | map(1:num_players(game)) do ii 190 | (x, t) -> zeros(control_dim(game.dynamics, ii)) 191 | end |> TrajectoryGamesBase.JointStrategy 192 | 193 | TrajectoryGamesBase.rollout(game.dynamics, rollout_strategy, initial_state, horizon) 194 | end 195 | 196 | "Solve a parametric trajectory game, where the parameter is just the initial state." 197 | function TrajectoryGamesBase.solve_trajectory_game!( 198 | game::TrajectoryGame{<:ProductDynamics}, 199 | horizon, 200 | parameter_value, 201 | strategy; 202 | parametric_game = build_parametric_game(; 203 | game, 204 | horizon, 205 | params_per_player = Int( 206 | (length(parameter_value) - state_dim(game)) / num_players(game), 207 | ), 208 | ), 209 | ) 210 | # Solve, maybe with warm starting. 211 | if !isnothing(strategy.last_solution) && strategy.last_solution.status == :solved 212 | solution = MixedComplementarityProblems.solve( 213 | parametric_game, 214 | parameter_value; 215 | solver_type = MixedComplementarityProblems.InteriorPoint(), 216 | x₀ = strategy.last_solution.variables.x, 217 | y₀ = strategy.last_solution.variables.y, 218 | ) 219 | else 220 | (; initial_state) = unpack_parameters(parameter_value; game.dynamics) 221 | solution = MixedComplementarityProblems.solve( 222 | parametric_game, 223 | parameter_value; 224 | solver_type = MixedComplementarityProblems.InteriorPoint(), 225 | x₀ = [ 226 | pack_trajectory(zero_input_trajectory(; game, horizon, initial_state)) 227 | zeros(sum(parametric_game.dims.λ) + parametric_game.dims.λ̃) 228 | ], 229 | ) 230 | end 231 | 232 | # Update warm starting info. 233 | if solution.status == :solved 234 | strategy.last_solution = solution 235 | end 236 | strategy.solution_status = solution.status 237 | 238 | # Pack solution into OpenLoopStrategy. 239 | trajs = unstack_trajectory(unpack_trajectory(mortar(solution.primals); game.dynamics)) 240 | JointStrategy(map(traj -> OpenLoopStrategy(traj.xs, traj.us), trajs)) 241 | end 242 | 243 | "Receding horizon strategy that supports warm starting." 244 | Base.@kwdef mutable struct WarmStartRecedingHorizonStrategy 245 | game::TrajectoryGame 246 | parametric_game::MixedComplementarityProblems.ParametricGame 247 | receding_horizon_strategy::Any = nothing 248 | time_last_updated::Int = 0 249 | turn_length::Int 250 | horizon::Int 251 | last_solution::Any = nothing 252 | parameters::Any = nothing 253 | solution_status::Any = nothing 254 | end 255 | 256 | function (strategy::WarmStartRecedingHorizonStrategy)(state, time) 257 | plan_exists = !isnothing(strategy.receding_horizon_strategy) 258 | time_along_plan = time - strategy.time_last_updated + 1 259 | plan_is_still_valid = 1 <= time_along_plan <= strategy.turn_length 260 | 261 | update_plan = !plan_exists || !plan_is_still_valid 262 | if update_plan 263 | strategy.receding_horizon_strategy = TrajectoryGamesBase.solve_trajectory_game!( 264 | strategy.game, 265 | strategy.horizon, 266 | pack_parameters(state, strategy.parameters), 267 | strategy; 268 | strategy.parametric_game, 269 | ) 270 | strategy.time_last_updated = time 271 | time_along_plan = 1 272 | end 273 | 274 | strategy.receding_horizon_strategy(state, time_along_plan) 275 | end 276 | --------------------------------------------------------------------------------