From 52d049d242f54673049999c4a0efb2f966f3d0f8 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Wed, 20 Aug 2025 17:57:56 -0700 Subject: [PATCH] linear, cone, equality, bound constraints with rocket example --- README.md | 21 ++- deps/build.log | 70 +++----- examples/cartpole_example_code_generation.jl | 4 +- examples/rocket_landing_constraints.jl | 171 +++++++++++++++++++ src/TinyMPC.jl | 138 ++++++++++----- src/bindings.cpp | 115 ++++++++++--- 6 files changed, 398 insertions(+), 121 deletions(-) create mode 100644 examples/rocket_landing_constraints.jl diff --git a/README.md b/README.md index 5151ee9..2b8359f 100644 --- a/README.md +++ b/README.md @@ -104,13 +104,32 @@ states_trajectory = solution.states # All predicted states (4×20) controls_trajectory = solution.controls # All predicted controls (1×19) ``` +### Constraints API + +Constraints are set after setup using dedicated functions. Each call auto-enables the corresponding flags in the C++ core. + +``` +# Bounds (nx×N, nx×N, nu×(N-1), nu×(N-1)) +set_bound_constraints(solver, x_min, x_max, u_min, u_max) + +# Linear inequalities: Alin_x x ≤ blin_x, Alin_u u ≤ blin_u +set_linear_constraints(solver, Alin_x, blin_x, Alin_u, blin_u) + +# Equalities via two inequalities +set_equality_constraints(solver, Aeq_x, beq_x; Aeq_u=Aeq_u, beq_u=beq_u) + +# Second-order cones (inputs first, then states) +set_cone_constraints(solver, Acu, qcu, cu, Acx, qcx, cx) +``` + ### Code Generation Workflow ```julia # Setup solver with constraints solver = TinyMPCSolver() u_min = fill(-0.5, 1, N-1); u_max = fill(0.5, 1, N-1) # Control bounds (1×19) -setup(solver, A, B, zeros(4), Q, R, rho, 4, 1, N, u_min=u_min, u_max=u_max) +setup(solver, A, B, zeros(4), Q, R, rho, 4, 1, N) +set_bound_constraints(solver, fill(-0.5, 1, N), fill(0.5, 1, N), u_min, u_max) # Generate C++ code codegen(solver, "out") diff --git a/deps/build.log b/deps/build.log index 30922d7..f8f2158 100644 --- a/deps/build.log +++ b/deps/build.log @@ -1,58 +1,32 @@ --- Found Julia: /home/imahajan/safe-reachability/julia-1.11.3/bin/julia (version 1.11.3) --- Julia include dir: /home/imahajan/safe-reachability/julia-1.11.3/include/julia --- Julia libraries: /home/imahajan/safe-reachability/julia-1.11.3/lib/libjulia.so --- Julia executable: /home/imahajan/safe-reachability/julia-1.11.3/bin/julia --- Julia include dir: /home/imahajan/safe-reachability/julia-1.11.3/include/julia --- Julia libraries: /home/imahajan/safe-reachability/julia-1.11.3/lib/libjulia.so +-- Found Julia: /home/moises/.juliaup/bin/julia (version 1.11.6) +-- Julia include dir: /home/moises/.julia/juliaup/julia-1.11.6+0.x64.linux.gnu/include/julia +-- Julia libraries: /home/moises/.julia/juliaup/julia-1.11.6+0.x64.linux.gnu/lib/libjulia.so +-- Julia executable: /home/moises/.juliaup/bin/julia +-- Julia include dir: /home/moises/.julia/juliaup/julia-1.11.6+0.x64.linux.gnu/include/julia +-- Julia libraries: /home/moises/.julia/juliaup/julia-1.11.6+0.x64.linux.gnu/lib/libjulia.so -- TinyMPC Julia wrapper configured successfully -- - Using direct Julia C API (no CxxWrap) -- - Target: libtinympc_jl.so -- - Build with: make -j${nproc} --- Configuring done --- Generating done --- Build files have been written to: /home/imahajan/tinympc-julia/build -[ 4%] Building CXX object tinympc/TinyMPC/src/tinympc/CMakeFiles/tinympcstatic.dir/rho_benchmark.cpp.o -[ 9%] Building CXX object tinympc/TinyMPC/src/tinympc/CMakeFiles/tinympcstatic.dir/codegen.cpp.o -[ 14%] Building CXX object tinympc/TinyMPC/src/tinympc/CMakeFiles/tinympcstatic.dir/tiny_api.cpp.o -[ 19%] Building CXX object tinympc/TinyMPC/src/tinympc/CMakeFiles/tinympcstatic.dir/admm.cpp.o -[ 23%] Linking CXX static library libtinympcstatic.a +-- Configuring done (0.4s) +-- Generating done (0.0s) +-- Build files have been written to: /home/moises/Documents/A2R/tinympc-wrappers/tinympc-julia/build [ 23%] Built target tinympcstatic -Consolidate compiler generated dependencies of target quadrotor_tracking -Consolidate compiler generated dependencies of target quadrotor_hovering -Consolidate compiler generated dependencies of target codegen_random -Consolidate compiler generated dependencies of target codegen_cartpole -Consolidate compiler generated dependencies of target tinympc_jl -[ 28%] Building CXX object tinympc/TinyMPC/examples/CMakeFiles/cartpole_example.dir/cartpole_example.cpp.o -[ 33%] Building CXX object tinympc/TinyMPC/examples/CMakeFiles/rocket_landing_mpc.dir/rocket_landing_mpc.cpp.o -[ 42%] Building CXX object tinympc/TinyMPC/examples/CMakeFiles/codegen_cartpole.dir/codegen_cartpole.cpp.o -[ 42%] Building CXX object tinympc/TinyMPC/examples/CMakeFiles/codegen_random.dir/codegen_random.cpp.o -[ 47%] Building CXX object tinympc/TinyMPC/examples/CMakeFiles/quadrotor_linear_constraints.dir/quadrotor_linear_constraints.cpp.o -[ 52%] Building CXX object tinympc/TinyMPC/examples/CMakeFiles/quadrotor_hovering.dir/quadrotor_hovering.cpp.o -[ 57%] Building CXX object tinympc/TinyMPC/examples/CMakeFiles/quadrotor_tracking.dir/quadrotor_tracking.cpp.o -[ 61%] Building CXX object CMakeFiles/tinympc_jl.dir/src/bindings.cpp.o -[ 66%] Linking CXX executable codegen_random -[ 71%] Linking CXX shared library tinympc_jl.so -[ 76%] Linking CXX executable codegen_cartpole -[ 76%] Built target codegen_random -[ 76%] Built target tinympc_jl -[ 76%] Built target codegen_cartpole -[ 80%] Linking CXX executable cartpole_example -[ 85%] Linking CXX executable quadrotor_hovering -[ 90%] Linking CXX executable quadrotor_linear_constraints -[ 95%] Linking CXX executable quadrotor_tracking -[ 95%] Built target cartpole_example -[ 95%] Built target quadrotor_hovering -[100%] Linking CXX executable rocket_landing_mpc +[ 33%] Built target quadrotor_hovering +[ 42%] Built target tinympc_jl +[ 52%] Built target quadrotor_tracking +[ 61%] Built target cartpole_example +[ 71%] Built target rocket_landing_mpc +[ 80%] Built target codegen_cartpole +[ 90%] Built target codegen_random [100%] Built target quadrotor_linear_constraints -[100%] Built target quadrotor_tracking -[100%] Built target rocket_landing_mpc Building TinyMPC Julia wrapper... -Package root: /home/imahajan/tinympc-julia -Build directory: /home/imahajan/tinympc-julia/build -Lib directory: /home/imahajan/tinympc-julia/lib +Package root: /home/moises/Documents/A2R/tinympc-wrappers/tinympc-julia +Build directory: /home/moises/Documents/A2R/tinympc-wrappers/tinympc-julia/build +Lib directory: /home/moises/Documents/A2R/tinympc-wrappers/tinympc-julia/lib Configuring with CMake... -Running in /home/imahajan/tinympc-julia/build: ``cmake ..`` +Running in /home/moises/Documents/A2R/tinympc-wrappers/tinympc-julia/build: ``cmake ..`` Building with make... -Running in /home/imahajan/tinympc-julia/build: ``make -j24`` -Copying library from /home/imahajan/tinympc-julia/build/tinympc_jl.so to /home/imahajan/tinympc-julia/lib/libtinympc_jl.so +Running in /home/moises/Documents/A2R/tinympc-wrappers/tinympc-julia/build: ``make -j22`` +Copying library from /home/moises/Documents/A2R/tinympc-wrappers/tinympc-julia/build/tinympc_jl.so to /home/moises/Documents/A2R/tinympc-wrappers/tinympc-julia/lib/libtinympc_jl.so TinyMPC Julia wrapper built successfully! diff --git a/examples/cartpole_example_code_generation.jl b/examples/cartpole_example_code_generation.jl index 7480051..988ce56 100644 --- a/examples/cartpole_example_code_generation.jl +++ b/examples/cartpole_example_code_generation.jl @@ -20,9 +20,9 @@ u_max = fill(0.5, 1, N-1) # nu x (N-1) # Create solver and setup with bounds solver = TinyMPCSolver() -status = setup(solver, A, B, zeros(4), Q, R, rho, 4, 1, N, - u_min=u_min, u_max=u_max, verbose=false) +status = setup(solver, A, B, zeros(4), Q, R, rho, 4, 1, N, verbose=false) @assert status == 0 +set_bound_constraints(solver, fill(-Inf, 4, N), fill(Inf, 4, N), u_min, u_max) # Set references (all zeros for code generation) set_x_ref(solver, zeros(4, N)) diff --git a/examples/rocket_landing_constraints.jl b/examples/rocket_landing_constraints.jl new file mode 100644 index 0000000..9f4f1c9 --- /dev/null +++ b/examples/rocket_landing_constraints.jl @@ -0,0 +1,171 @@ +""" +Rocket Landing with Constraints +Based on: https://github.com/TinyMPC/TinyMPC/blob/main/examples/rocket_landing_mpc.cpp +""" + +using TinyMPC +using LinearAlgebra +using Plots +using Printf + +# Problem dimensions +const NSTATES = 6 # [x, y, z, vx, vy, vz] +const NINPUTS = 3 # [thrust_x, thrust_y, thrust_z] +const NHORIZON = 10 + +# System dynamics (from rocket_landing_params_20hz.hpp) +A = [1.0 0.0 0.0 0.05 0.0 0.0; + 0.0 1.0 0.0 0.0 0.05 0.0; + 0.0 0.0 1.0 0.0 0.0 0.05; + 0.0 0.0 0.0 1.0 0.0 0.0; + 0.0 0.0 0.0 0.0 1.0 0.0; + 0.0 0.0 0.0 0.0 0.0 1.0] + +B = [0.000125 0.0 0.0; + 0.0 0.000125 0.0; + 0.0 0.0 0.000125; + 0.005 0.0 0.0; + 0.0 0.005 0.0; + 0.0 0.0 0.005] + +fdyn = [0.0, 0.0, -0.0122625, 0.0, 0.0, -0.4905] +Q = diagm([101.0, 101.0, 101.0, 101.0, 101.0, 101.0]) # From parameter file +R = diagm([2.0, 2.0, 2.0]) # From parameter file + +# Box constraints +x_min = fill(-1e17, NSTATES, NHORIZON) +x_max = fill(1e17, NSTATES, NHORIZON) +u_min = fill(-1e17, NINPUTS, NHORIZON-1) +u_max = fill(1e17, NINPUTS, NHORIZON-1) + +# Apply specific bounds +x_min[1, :] .= -5.0; x_max[1, :] .= 5.0 # x position +x_min[2, :] .= -5.0; x_max[2, :] .= 5.0 # y position +x_min[3, :] .= -0.5; x_max[3, :] .= 100.0 # z position +x_min[4, :] .= -10.0; x_max[4, :] .= 10.0 # x velocity +x_min[5, :] .= -10.0; x_max[5, :] .= 10.0 # y velocity +x_min[6, :] .= -20.0; x_max[6, :] .= 20.0 # z velocity + +u_min .= -10.0; u_max .= 105.0 # thrust limits + +# SOC constraints +cx = [0.5] # coefficients for state cones (mu) +cu = [0.25] # coefficients for input cones (mu) +Acx = [0] # start indices for state cones (0-indexed for C++) +Acu = [0] # start indices for input cones (0-indexed for C++) +qcx = [3] # dimensions for state cones +qcu = [3] # dimensions for input cones + +# Setup solver +solver = TinyMPCSolver() +setup(solver, A, B, fdyn, Q, R, 1.0, NSTATES, NINPUTS, NHORIZON, + verbose=true, max_iter=100, abs_pri_tol=2e-3, abs_dua_tol=1e-3) + +# Set bound constraints explicitly, auto-enables flags +set_bound_constraints(solver, x_min, x_max, u_min, u_max) + +# Set cone constraints (inputs first), auto-enables flags +set_cone_constraints(solver, Int32.(Acu), Int32.(qcu), cu, Int32.(Acx), Int32.(qcx), cx, verbose=true) + +# Initial and goal states +xinit = [4.0, 2.0, 20.0, -3.0, 2.0, -4.5] +xgoal = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + +# Initial reference trajectory (will be updated each step like C++) +x_ref = zeros(NSTATES, NHORIZON) +u_ref = zeros(NINPUTS, NHORIZON-1) + +# Animation setup - Extended for longer simulation +const NTOTAL = 100 # Match C++ +x_current = xinit * 1.1 # Match C++ (x0 = xinit * 1.1) + +# Set initial reference +for i in 1:NHORIZON + x_ref[:, i] = xinit + (xgoal - xinit) * (i-1) / (NTOTAL - 1) # Use NTOTAL-1 like C++ +end +u_ref[3, :] .= 10.0 # Hover thrust + +set_x_ref(solver, x_ref) +set_u_ref(solver, u_ref) + +# Store trajectory for plotting +trajectory = [] +controls = [] +constraint_violations = [] + +println("Starting rocket landing simulation...") +for k in 1:(NTOTAL - NHORIZON) + global x_current # Declare as global to modify in loop + + # Calculate tracking error (match C++ exactly: (x0 - Xref.col(1)).norm()) + tracking_error = norm(x_current - x_ref[:, 2]) + @printf("tracking error: %.5f\n", tracking_error) + + # 1. Update measurement (set current state) + set_x0(solver, x_current) + + # 2. Update reference trajectory (match C++ exactly) + for i in 1:NHORIZON + x_ref[:, i] = xinit + (xgoal - xinit) * (i + k - 2) / (NTOTAL - 1) + if i <= NHORIZON - 1 + u_ref[3, i] = 10.0 # uref stays constant + end + end + + set_x_ref(solver, x_ref) + set_u_ref(solver, u_ref) + + # 3. Solve MPC problem + status = solve(solver) + solution = get_solution(solver) + + # 4. Simulate forward (apply first control) + u_opt = solution.controls[:, 1] + x_current = A * x_current + B * u_opt + fdyn + + # Store data for plotting + push!(trajectory, copy(x_current)) + push!(controls, copy(u_opt)) + + # Check constraint violations + altitude_violation = x_current[3] < 0 # Ground constraint + thrust_violation = norm(u_opt[1:2]) > 0.25 * abs(u_opt[3]) # Cone constraint + push!(constraint_violations, altitude_violation || thrust_violation) +end + +# Convert to matrices for plotting +traj_matrix = hcat(trajectory...) +ctrl_matrix = hcat(controls...) + +println("\nSimulation completed!") +@printf("Initial state was: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f]\n", (xinit * 1.1)...) +@printf("Final position: [%.2f, %.2f, %.2f]\n", x_current[1:3]...) +@printf("Final velocity: [%.2f, %.2f, %.2f]\n", x_current[4:6]...) +@printf("Distance to goal: %.3f m\n", norm(x_current[1:3])) +@printf("Constraint violations: %d/%d\n", sum(constraint_violations), length(constraint_violations)) + +# Plotting +time_steps = 1:size(traj_matrix, 2) + +p1 = plot(traj_matrix[1, :], traj_matrix[2, :], lw=2, label="Trajectory", + title="2D Trajectory (X-Y)", xlabel="X (m)", ylabel="Y (m)") +scatter!(p1, [(xinit * 1.1)[1]], [(xinit * 1.1)[2]], c=:red, ms=8, label="Start") +scatter!(p1, [xgoal[1]], [xgoal[2]], c=:green, ms=8, label="Goal") + +p2 = plot(time_steps, traj_matrix[1, :], label="X", lw=1.5, c=:red, + title="Position vs Time", xlabel="Time Step", ylabel="Position (m)") +plot!(p2, time_steps, traj_matrix[2, :], label="Y", lw=1.5, c=:green) +plot!(p2, time_steps, traj_matrix[3, :], label="Z", lw=1.5, c=:blue) +hline!(p2, [0], ls=:dash, c=:black, alpha=0.5, label="Ground") + +p3 = plot(time_steps, traj_matrix[4, :], label="Vx", lw=1.5, c=:red, + title="Velocity vs Time", xlabel="Time Step", ylabel="Velocity (m/s)") +plot!(p3, time_steps, traj_matrix[5, :], label="Vy", lw=1.5, c=:green) +plot!(p3, time_steps, traj_matrix[6, :], label="Vz", lw=1.5, c=:blue) + +p4 = plot(time_steps, ctrl_matrix[1, :], label="Thrust X", lw=1.5, c=:red, + title="Thrust vs Time", xlabel="Time Step", ylabel="Thrust (N)") +plot!(p4, time_steps, ctrl_matrix[2, :], label="Thrust Y", lw=1.5, c=:green) +plot!(p4, time_steps, ctrl_matrix[3, :], label="Thrust Z", lw=1.5, c=:blue) + +plot(p1, p2, p3, p4, layout=(2,2), size=(1200, 900)) diff --git a/src/TinyMPC.jl b/src/TinyMPC.jl index d31c0ce..a70c2bb 100644 --- a/src/TinyMPC.jl +++ b/src/TinyMPC.jl @@ -1,7 +1,8 @@ module TinyMPC -export TinyMPCSolver, setup, solve, get_solution, set_x0, set_x_ref, set_u_ref, - set_bound_constraints, update_settings, set_cache_terms, print_problem_data, +export TinyMPCSolver, setup, solve, get_solution, set_x0, set_x_ref, set_u_ref, + set_bound_constraints, set_linear_constraints, set_cone_constraints, set_equality_constraints, + update_settings, set_cache_terms, print_problem_data, compute_sensitivity_autograd, codegen, codegen_with_sensitivity using LinearAlgebra, Libdl, Printf @@ -54,21 +55,13 @@ Setup the MPC problem with system matrices and parameters. function setup(solver::TinyMPCSolver, A::Matrix{Float64}, B::Matrix{Float64}, f::Vector{Float64}, Q::Matrix{Float64}, R::Matrix{Float64}, rho::Float64, nx::Int, nu::Int, N::Int; verbose::Bool=false, abs_pri_tol::Float64=1e-3, abs_dua_tol::Float64=1e-3, - max_iter::Int=100, check_termination::Bool=true, enable_state_bound::Bool=false, - enable_input_bound::Bool=false, adaptive_rho::Bool=false, + max_iter::Int=100, check_termination::Bool=true, + adaptive_rho::Bool=false, adaptive_rho_min::Float64=0.1, adaptive_rho_max::Float64=10.0, - adaptive_rho_clipping::Bool=true, - x_min::Union{Matrix{Float64}, Nothing}=nothing, x_max::Union{Matrix{Float64}, Nothing}=nothing, - u_min::Union{Matrix{Float64}, Nothing}=nothing, u_max::Union{Matrix{Float64}, Nothing}=nothing) + adaptive_rho_clipping::Bool=true) _ensure_loaded() - # Set default bounds if not provided - x_min = x_min === nothing ? fill(-1e17, nx, N) : x_min - x_max = x_max === nothing ? fill(1e17, nx, N) : x_max - u_min = u_min === nothing ? fill(-1e17, nu, N-1) : u_min - u_max = u_max === nothing ? fill(1e17, nu, N-1) : u_max - f_matrix = reshape(f, nx, 1) # Store problem data in solver @@ -84,25 +77,26 @@ function setup(solver::TinyMPCSolver, A::Matrix{Float64}, B::Matrix{Float64}, f: status = ccall((:setup_solver, _lib_path()), Int32, (Ptr{Float64}, Int32, Int32, Ptr{Float64}, Int32, Int32, Ptr{Float64}, Int32, Int32, Ptr{Float64}, Int32, Int32, Ptr{Float64}, Int32, Int32, Float64, Int32, Int32, Int32, - Ptr{Float64}, Int32, Int32, Ptr{Float64}, Int32, Int32, Ptr{Float64}, Int32, Int32, Ptr{Float64}, Int32, Int32, Int32), A, size(A,1), size(A,2), B, size(B,1), size(B,2), f_matrix, size(f_matrix,1), size(f_matrix,2), Q, size(Q,1), size(Q,2), R, size(R,1), size(R,2), rho, nx, nu, N, - x_min, size(x_min,1), size(x_min,2), x_max, size(x_max,1), size(x_max,2), - u_min, size(u_min,1), size(u_min,2), u_max, size(u_max,1), size(u_max,2), verbose ? 1 : 0) if status == 0 solver.is_setup[] = true # Push settings to C++ layer (including adaptive_rho settings) - update_settings(solver, - abs_pri_tol=abs_pri_tol, + update_settings(solver, + abs_pri_tol=abs_pri_tol, abs_dua_tol=abs_dua_tol, - max_iter=max_iter, + max_iter=max_iter, check_termination=check_termination, - en_state_bound=enable_state_bound, - en_input_bound=enable_input_bound, + en_state_bound=false, + en_input_bound=false, + en_state_soc=false, + en_input_soc=false, + en_state_linear=false, + en_input_linear=false, adaptive_rho=adaptive_rho, adaptive_rho_min=adaptive_rho_min, adaptive_rho_max=adaptive_rho_max, @@ -150,7 +144,6 @@ function solve(solver::TinyMPCSolver; verbose::Bool=false) solver.is_setup[] || error("Solver not setup") _ensure_loaded() status = ccall((:solve_mpc, _lib_path()), Int32, (Int32,), verbose ? 1 : 0) - status != 0 && error("Solver failed") return status end @@ -183,9 +176,44 @@ function get_solution(solver::TinyMPCSolver) return (states=states, controls=controls) end -function set_bound_constraints(solver::TinyMPCSolver, - x_min::Matrix{Float64}, x_max::Matrix{Float64}, - u_min::Matrix{Float64}, u_max::Matrix{Float64}; verbose::Bool=false) +## Remove old duplicate definition (consolidated below) + +function update_settings(solver::TinyMPCSolver; + abs_pri_tol::Float64=1e-3, + abs_dua_tol::Float64=1e-3, + max_iter::Int=100, + check_termination::Bool=true, + en_state_bound::Bool=false, + en_input_bound::Bool=false, + en_state_soc::Bool=false, + en_input_soc::Bool=false, + en_state_linear::Bool=false, + en_input_linear::Bool=false, + adaptive_rho::Bool=false, + adaptive_rho_min::Float64=0.1, + adaptive_rho_max::Float64=10.0, + adaptive_rho_enable_clipping::Bool=true, + verbose::Bool=false) + _ensure_loaded() + + status = ccall((:update_settings, _lib_path()), Int32, + (Float64, Float64, Int32, Int32, Int32, Int32, Int32, Int32, Int32, Int32, + Int32, Float64, Float64, Int32, Int32), + abs_pri_tol, abs_dua_tol, max_iter, check_termination ? 1 : 0, + en_state_bound ? 1 : 0, en_input_bound ? 1 : 0, + en_state_soc ? 1 : 0, en_input_soc ? 1 : 0, + en_state_linear ? 1 : 0, en_input_linear ? 1 : 0, + adaptive_rho ? 1 : 0, adaptive_rho_min, adaptive_rho_max, + adaptive_rho_enable_clipping ? 1 : 0, verbose ? 1 : 0) + + status != 0 && error("Failed to update settings") + return status +end + +# Constraints API +function set_bound_constraints(solver::TinyMPCSolver, + x_min::Matrix{Float64}, x_max::Matrix{Float64}, + u_min::Matrix{Float64}, u_max::Matrix{Float64}; verbose::Bool=false) _ensure_loaded() status = ccall((:set_bound_constraints, _lib_path()), Int32, (Ptr{Float64}, Int32, Int32, Ptr{Float64}, Int32, Int32, @@ -194,35 +222,51 @@ function set_bound_constraints(solver::TinyMPCSolver, u_min, size(u_min,1), size(u_min,2), u_max, size(u_max,1), size(u_max,2), verbose ? 1 : 0) status != 0 && error("Failed to set bound constraints") + # Flags auto-enabled in C++ binding return status end -function update_settings(solver::TinyMPCSolver; - abs_pri_tol::Float64=1e-3, - abs_dua_tol::Float64=1e-3, - max_iter::Int=100, - check_termination::Bool=true, - en_state_bound::Bool=false, - en_input_bound::Bool=false, - adaptive_rho::Bool=false, - adaptive_rho_min::Float64=0.1, - adaptive_rho_max::Float64=10.0, - adaptive_rho_enable_clipping::Bool=true, - verbose::Bool=false) +function set_linear_constraints(solver::TinyMPCSolver, + Alin_x::Matrix{Float64}, blin_x::Vector{Float64}, + Alin_u::Matrix{Float64}, blin_u::Vector{Float64}; verbose::Bool=false) _ensure_loaded() - - status = ccall((:update_settings, _lib_path()), Int32, - (Float64, Float64, Int32, Int32, Int32, Int32, - Int32, Float64, Float64, Int32, Int32), - abs_pri_tol, abs_dua_tol, max_iter, check_termination ? 1 : 0, - en_state_bound ? 1 : 0, en_input_bound ? 1 : 0, - adaptive_rho ? 1 : 0, adaptive_rho_min, adaptive_rho_max, - adaptive_rho_enable_clipping ? 1 : 0, verbose ? 1 : 0) - - status != 0 && error("Failed to update settings") + status = ccall((:set_linear_constraints, _lib_path()), Int32, + (Ptr{Float64}, Int32, Int32, Ptr{Float64}, Int32, + Ptr{Float64}, Int32, Int32, Ptr{Float64}, Int32, Int32), + Alin_x, size(Alin_x,1), size(Alin_x,2), blin_x, length(blin_x), + Alin_u, size(Alin_u,1), size(Alin_u,2), blin_u, length(blin_u), + verbose ? 1 : 0) + status != 0 && error("Failed to set linear constraints") + # Flags auto-enabled in C++ binding + return status +end + +function set_cone_constraints(solver::TinyMPCSolver, + Acu::Vector{Int32}, qcu::Vector{Int32}, cu::Vector{Float64}, + Acx::Vector{Int32}, qcx::Vector{Int32}, cx::Vector{Float64}; verbose::Bool=false) + _ensure_loaded() + status = ccall((:set_cone_constraints, _lib_path()), Int32, + (Ptr{Int32}, Int32, Ptr{Int32}, Int32, Ptr{Float64}, Int32, + Ptr{Int32}, Int32, Ptr{Int32}, Int32, Ptr{Float64}, Int32, Int32), + Acu, length(Acu), qcu, length(qcu), cu, length(cu), + Acx, length(Acx), qcx, length(qcx), cx, length(cx), + verbose ? 1 : 0) + status != 0 && error("Failed to set cone constraints") + # Flags auto-enabled in C++ binding return status end +function set_equality_constraints(solver::TinyMPCSolver, + Aeq_x::Matrix{Float64}, beq_x::Vector{Float64}; + Aeq_u::Matrix{Float64}=zeros(0, size(solver.B[],2)), beq_u::Vector{Float64}=zeros(Float64, 0)) + # Implement equalities via two inequalities and delegate + Alin_x = vcat(Aeq_x, -Aeq_x) + blin_x = vcat(beq_x, -beq_x) + Alin_u = vcat(Aeq_u, -Aeq_u) + blin_u = vcat(beq_u, -beq_u) + return set_linear_constraints(solver, Alin_x, blin_x, Alin_u, blin_u) +end + function print_problem_data(solver::TinyMPCSolver; verbose::Bool=false) solver.is_setup[] || error("Solver not setup") _ensure_loaded() diff --git a/src/bindings.cpp b/src/bindings.cpp index c502da0..95e1b53 100644 --- a/src/bindings.cpp +++ b/src/bindings.cpp @@ -17,17 +17,13 @@ static std::unique_ptr g_solver = nullptr; // Global problem dimensions for array extraction static int g_nx = 0, g_nu = 0, g_N = 0; -// Setup function - initialize the solver +// Setup function - initialize the solver (constraints are set separately) extern "C" int setup_solver(double* A_data, int A_rows, int A_cols, double* B_data, int B_rows, int B_cols, double* fdyn_data, int fdyn_rows, int fdyn_cols, double* Q_data, int Q_rows, int Q_cols, double* R_data, int R_rows, int R_cols, double rho, int nx, int nu, int N, - double* x_min_data, int x_min_rows, int x_min_cols, - double* x_max_data, int x_max_rows, int x_max_cols, - double* u_min_data, int u_min_rows, int u_min_cols, - double* u_max_data, int u_max_rows, int u_max_cols, int verbose) { try { if (verbose) { @@ -46,11 +42,6 @@ extern "C" int setup_solver(double* A_data, int A_rows, int A_cols, Eigen::Map fdyn(fdyn_data, fdyn_rows, fdyn_cols); Eigen::Map Q(Q_data, Q_rows, Q_cols); Eigen::Map R(R_data, R_rows, R_cols); - Eigen::Map x_min(x_min_data, x_min_rows, x_min_cols); - Eigen::Map x_max(x_max_data, x_max_rows, x_max_cols); - Eigen::Map u_min(u_min_data, u_min_rows, u_min_cols); - Eigen::Map u_max(u_max_data, u_max_rows, u_max_cols); - // Create solver pointer for tiny_setup TinySolver* solver_ptr = nullptr; @@ -68,18 +59,6 @@ extern "C" int setup_solver(double* A_data, int A_rows, int A_cols, return status; } - // Set bound constraints using correct API - status = tiny_set_bound_constraints(solver_ptr, - x_min.cast(), - x_max.cast(), - u_min.cast(), - u_max.cast()); - - if (status != 0) { - delete solver_ptr; - return status; - } - // Store solver (transfer ownership) g_solver.reset(solver_ptr); @@ -357,6 +336,8 @@ extern "C" int codegen_with_sensitivity(const char* output_dir, extern "C" int update_settings(double abs_pri_tol, double abs_dua_tol, int max_iter, int check_termination, int en_state_bound, int en_input_bound, + int en_state_soc, int en_input_soc, + int en_state_linear, int en_input_linear, int adaptive_rho, double adaptive_rho_min, double adaptive_rho_max, int adaptive_rho_enable_clipping, int verbose) { @@ -372,6 +353,10 @@ extern "C" int update_settings(double abs_pri_tol, double abs_dua_tol, g_solver->settings->check_termination = check_termination; g_solver->settings->en_state_bound = en_state_bound; g_solver->settings->en_input_bound = en_input_bound; + g_solver->settings->en_state_soc = en_state_soc; + g_solver->settings->en_input_soc = en_input_soc; + g_solver->settings->en_state_linear = en_state_linear; + g_solver->settings->en_input_linear = en_input_linear; // Update adaptive rho settings g_solver->settings->adaptive_rho = adaptive_rho; @@ -412,10 +397,94 @@ extern "C" int set_bound_constraints(double* x_min_data, int x_min_rows, int x_m u_max.cast()); if (verbose) std::cout << "set_bound_constraints status: " << status << std::endl; + if (status == 0) { + // Auto-enable bound flags + g_solver->settings->en_state_bound = 1; + g_solver->settings->en_input_bound = 1; + } return status; } catch (const std::exception& e) { std::cerr << "set_bound_constraints failed: " << e.what() << std::endl; return -1; } -} \ No newline at end of file +} + +// Set linear constraints with auto-enable +extern "C" int set_linear_constraints(double* Alin_x_data, int Alin_x_rows, int Alin_x_cols, + double* blin_x_data, int blin_x_len, + double* Alin_u_data, int Alin_u_rows, int Alin_u_cols, + double* blin_u_data, int blin_u_len, + int verbose) { + try { + if (!g_solver) { + throw std::runtime_error("Solver not initialized"); + } + + Eigen::Map Alin_x(Alin_x_data, Alin_x_rows, Alin_x_cols); + Eigen::Map blin_x_in(blin_x_data, blin_x_len, 1); + Eigen::Map Alin_u(Alin_u_data, Alin_u_rows, Alin_u_cols); + Eigen::Map blin_u_in(blin_u_data, blin_u_len, 1); + + Eigen::VectorXd blin_x_vec = Eigen::Map(blin_x_in.data(), (int)blin_x_in.size()); + Eigen::VectorXd blin_u_vec = Eigen::Map(blin_u_in.data(), (int)blin_u_in.size()); + + int status = tiny_set_linear_constraints( + g_solver.get(), + Alin_x.cast(), blin_x_vec.cast(), + Alin_u.cast(), blin_u_vec.cast()); + + if (verbose) std::cout << "set_linear_constraints status: " << status << std::endl; + if (status == 0) { + bool has_state_linear = (Alin_x_rows > 0 && blin_x_len > 0); + bool has_input_linear = (Alin_u_rows > 0 && blin_u_len > 0); + if (has_state_linear) g_solver->settings->en_state_linear = 1; + if (has_input_linear) g_solver->settings->en_input_linear = 1; + } + return status; + + } catch (const std::exception& e) { + std::cerr << "set_linear_constraints failed: " << e.what() << std::endl; + return -1; + } +} + +// Set SOC constraints with auto-enable (inputs first, then states) +extern "C" int set_cone_constraints(int* Acu_data, int Acu_len, + int* qcu_data, int qcu_len, + double* cu_data, int cu_len, + int* Acx_data, int Acx_len, + int* qcx_data, int qcx_len, + double* cx_data, int cx_len, + int verbose) { + try { + if (!g_solver) { + throw std::runtime_error("Solver not initialized"); + } + + Eigen::Map Acu(Acu_data, Acu_len); + Eigen::Map qcu(qcu_data, qcu_len); + Eigen::Map cu_in(cu_data, cu_len); + Eigen::Map Acx(Acx_data, Acx_len); + Eigen::Map qcx(qcx_data, qcx_len); + Eigen::Map cx_in(cx_data, cx_len); + + int status = tiny_set_cone_constraints( + g_solver.get(), + Acu, qcu, cu_in.cast(), + Acx, qcx, cx_in.cast()); + + if (verbose) std::cout << "set_cone_constraints status: " << status << std::endl; + if (status == 0) { + bool has_state_cones = (Acx_len > 0 && qcx_len > 0 && cx_len > 0); + bool has_input_cones = (Acu_len > 0 && qcu_len > 0 && cu_len > 0); + if (has_state_cones) g_solver->settings->en_state_soc = 1; + if (has_input_cones) g_solver->settings->en_input_soc = 1; + } + return status; + + } catch (const std::exception& e) { + std::cerr << "set_cone_constraints failed: " << e.what() << std::endl; + return -1; + } +} \ No newline at end of file