From bb36fd1a3867d380818ecebad1ca33aac1032e75 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Wed, 20 Aug 2025 11:13:44 -0700 Subject: [PATCH 1/5] Initial new constraints --- examples/rocket_landing_constraints.m | 183 ++++++++++++++++++++++++++ src/TinyMPC.m | 111 ++++++++++++++++ src/bindings.cpp | 107 +++++++++++++++ 3 files changed, 401 insertions(+) create mode 100644 examples/rocket_landing_constraints.m diff --git a/examples/rocket_landing_constraints.m b/examples/rocket_landing_constraints.m new file mode 100644 index 0000000..05dcffa --- /dev/null +++ b/examples/rocket_landing_constraints.m @@ -0,0 +1,183 @@ +%% Rocket Landing with Constraints +% Based on: https://github.com/TinyMPC/TinyMPC/blob/main/examples/rocket_landing_mpc.cpp +% MATLAB version of the Python rocket_landing_constraints.py example + +clear; clc; close all; + +% Add TinyMPC class to path (following existing examples) +currentFile = mfilename('fullpath'); +[scriptPath, ~, ~] = fileparts(currentFile); +repoRoot = fileparts(scriptPath); +addpath(fullfile(repoRoot, 'src')); +addpath(fullfile(repoRoot, 'build')); + +% Problem dimensions +NSTATES = 6; % [x, y, z, vx, vy, vz] +NINPUTS = 3; % [thrust_x, thrust_y, thrust_z] +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 = diag([101.0, 101.0, 101.0, 101.0, 101.0, 101.0]); % From parameter file +R = diag([2.0, 2.0, 2.0]); % From parameter file + +% Box constraints +x_min = [-5.0; -5.0; -0.5; -10.0; -10.0; -20.0]; +x_max = [5.0; 5.0; 100.0; 10.0; 10.0; 20.0]; +u_min = [-10.0; -10.0; -10.0]; +u_max = [105.0; 105.0; 105.0]; + +% 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 = TinyMPC(); +solver.setup(A, B, Q, R, NHORIZON, 'rho', 1.0, 'fdyn', fdyn, ... + 'x_min', x_min, 'x_max', x_max, 'u_min', u_min, 'u_max', u_max, ... + 'max_iter', 100, 'abs_pri_tol', 2e-3, 'verbose', true); + +% Set cone constraints +solver.set_cone_constraints(Acx, qcx, cx, Acu, qcu, cu); + +% Initial and goal states (matching corrected C++ version) +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]; +xinit_scaled = xinit * 1.1; + +% 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 +NTOTAL = 100; % Match C++ +x_current = xinit_scaled; % Match C++ (x0 = xinit * 1.1) + +% Set initial reference (match C++ initial setup) +% C++: work->Xref.col(i) = xinit + (xg - xinit)*tinytype(i)/(NTOTAL-1); +% C++ i: 0 to NHORIZON-1, so MATLAB equivalent: (i-1) +for i = 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 + +solver.set_x_ref(x_ref); +solver.set_u_ref(u_ref); + +% Store trajectory for plotting +trajectory = []; +controls = []; +constraint_violations = []; + +fprintf('Starting rocket landing simulation...\n'); +for k = 1:(NTOTAL - NHORIZON) + % Calculate tracking error (match C++ exactly: (x0 - Xref.col(1)).norm()) + tracking_error = norm(x_current - x_ref(:, 2)); % MATLAB is 1-indexed + fprintf('tracking error: %.5f\n', tracking_error); + + % 1. Update measurement (set current state) + solver.set_x0(x_current); + + % 2. Update reference trajectory (match C++ exactly) + % C++: work->Xref.col(i) = xinit + (xg - xinit)*tinytype(i+k)/(NTOTAL-1); + % C++ i: 0 to NHORIZON-1, k: 0 to NTOTAL-NHORIZON-1 + % MATLAB i: 1 to NHORIZON, k: 1 to NTOTAL-NHORIZON + % So MATLAB equivalent: (i-1)+(k-1) = i+k-2 + for i = 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 + + solver.set_x_ref(x_ref); + solver.set_u_ref(u_ref); + + % 3. Solve MPC problem + solver.solve(); + solution = solver.get_solution(); + + % 4. Simulate forward (apply first control) + u_opt = solution.controls(:, 1); + x_current = A * x_current + B * u_opt + fdyn; + + % Store data for plotting + trajectory = [trajectory, x_current]; + controls = [controls, 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 + constraint_violations = [constraint_violations, (altitude_violation || thrust_violation)]; +end + +fprintf('\nSimulation completed!\n'); +fprintf('Initial state was: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f]\n', xinit_scaled'); +fprintf('Final position: [%.2f, %.2f, %.2f]\n', x_current(1:3)'); +fprintf('Final velocity: [%.2f, %.2f, %.2f]\n', x_current(4:6)'); +fprintf('Distance to goal: %.3f m\n', norm(x_current(1:3))); +fprintf('Constraint violations: %d/%d\n', sum(constraint_violations), length(constraint_violations)); + +% Plotting +figure('Position', [100, 100, 1200, 900]); +sgtitle('Rocket Landing with Constraints', 'FontSize', 16); + +% 2D trajectory (X-Y view) +subplot(2, 2, 1); +plot(trajectory(1, :), trajectory(2, :), 'b-', 'LineWidth', 2); hold on; +scatter(xinit_scaled(1), xinit_scaled(2), 100, 'red', 'filled'); +scatter(xgoal(1), xgoal(2), 100, 'green', 'filled'); +xlabel('X (m)'); ylabel('Y (m)'); +legend('Trajectory', 'Start', 'Goal', 'Location', 'best'); +title('2D Trajectory (X-Y)'); +grid on; + +% Position vs time +subplot(2, 2, 2); +time_steps = 1:size(trajectory, 2); +plot(time_steps, trajectory(1, :), 'r-', 'LineWidth', 1.5); hold on; +plot(time_steps, trajectory(2, :), 'g-', 'LineWidth', 1.5); +plot(time_steps, trajectory(3, :), 'b-', 'LineWidth', 1.5); +yline(0, 'k--', 'Alpha', 0.5); +xlabel('Time Step'); ylabel('Position (m)'); +legend('X', 'Y', 'Z', 'Ground', 'Location', 'best'); +title('Position vs Time'); +grid on; + +% Velocity vs time +subplot(2, 2, 3); +plot(time_steps, trajectory(4, :), 'r-', 'LineWidth', 1.5); hold on; +plot(time_steps, trajectory(5, :), 'g-', 'LineWidth', 1.5); +plot(time_steps, trajectory(6, :), 'b-', 'LineWidth', 1.5); +xlabel('Time Step'); ylabel('Velocity (m/s)'); +legend('Vx', 'Vy', 'Vz', 'Location', 'best'); +title('Velocity vs Time'); +grid on; + +% Thrust vs time +subplot(2, 2, 4); +plot(time_steps, controls(1, :), 'r-', 'LineWidth', 1.5); hold on; +plot(time_steps, controls(2, :), 'g-', 'LineWidth', 1.5); +plot(time_steps, controls(3, :), 'b-', 'LineWidth', 1.5); +xlabel('Time Step'); ylabel('Thrust (N)'); +legend('Thrust X', 'Thrust Y', 'Thrust Z', 'Location', 'best'); +title('Thrust vs Time'); +grid on; diff --git a/src/TinyMPC.m b/src/TinyMPC.m index b5a2b2d..e94f598 100644 --- a/src/TinyMPC.m +++ b/src/TinyMPC.m @@ -256,6 +256,117 @@ function set_sensitivity_matrices(obj, dK, dP, dC1, dC2) dC2 = (C2_1 - C2_0) / h; end + function set_linear_constraints(obj, Alin_x, blin_x, Alin_u, blin_u) + % Set linear constraints: Alin_x * x <= blin_x, Alin_u * u <= blin_u + % + % Args: + % Alin_x (matrix): State constraint matrix (num_state_constraints x nx) + % blin_x (vector): State constraint bounds (num_state_constraints x 1) + % Alin_u (matrix): Input constraint matrix (num_input_constraints x nu) + % blin_u (vector): Input constraint bounds (num_input_constraints x 1) + obj.check_setup(); + + % Validate dimensions + if ~isempty(Alin_x) + assert(size(Alin_x, 2) == obj.nx, 'Alin_x must have nx columns'); + assert(size(blin_x, 1) == size(Alin_x, 1), 'blin_x must match Alin_x rows'); + assert(size(blin_x, 2) == 1, 'blin_x must be a column vector'); + else + Alin_x = zeros(0, obj.nx); + blin_x = zeros(0, 1); + end + + if ~isempty(Alin_u) + assert(size(Alin_u, 2) == obj.nu, 'Alin_u must have nu columns'); + assert(size(blin_u, 1) == size(Alin_u, 1), 'blin_u must match Alin_u rows'); + assert(size(blin_u, 2) == 1, 'blin_u must be a column vector'); + else + Alin_u = zeros(0, obj.nu); + blin_u = zeros(0, 1); + end + + % Call MEX function + tinympc_matlab('set_linear_constraints', Alin_x, blin_x, Alin_u, blin_u, false); + fprintf('Linear constraints set: %d state constraints, %d input constraints\n', ... + size(Alin_x, 1), size(Alin_u, 1)); + end + + function set_cone_constraints(obj, Acu, qcu, cu, Acx, qcx, cx) + % Set second-order cone constraints (inputs first, then states) + % + % Args: + % Acx (vector): Start indices for each state cone constraint (integer vector) + % qcx (vector): Dimension of each state cone constraint (integer vector) + % cx (vector): Cone parameter for each state cone constraint + % Acu (vector): Start indices for each input cone constraint (integer vector) + % qcu (vector): Dimension of each input cone constraint (integer vector) + % cu (vector): Cone parameter for each input cone constraint + obj.check_setup(); + + % Validate dimensions + if ~isempty(Acu) + assert(length(qcu) == length(Acu), 'qcu must match Acu length'); + assert(length(cu) == length(Acu), 'cu must match Acu length'); + else + Acu = []; + qcu = []; + cu = []; + end + + if ~isempty(Acx) + assert(length(qcx) == length(Acx), 'qcx must match Acx length'); + assert(length(cx) == length(Acx), 'cx must match Acx length'); + else + Acx = []; + qcx = []; + cx = []; + end + + % Ensure column vectors + Acu = Acu(:); qcu = qcu(:); cu = cu(:); + Acx = Acx(:); qcx = qcx(:); cx = cx(:); + + % Call MEX function + tinympc_matlab('set_cone_constraints', Acx, qcx, cx, Acu, qcu, cu, false); % MEX expects Acx first by historical order + fprintf('Cone constraints set: %d input cones, %d state cones\n', ... + length(Acu), length(Acx)); + end + + function set_equality_constraints(obj, Aeq_x, beq_x, Aeq_u, beq_u) + % Set equality constraints: Aeq_x * x == beq_x, Aeq_u * u == beq_u + % Implemented as two inequalities: <= and >= + % + % Args: + % Aeq_x (matrix): State equality constraint matrix (num_eq_state x nx) + % beq_x (vector): State equality constraint values (num_eq_state x 1) + % Aeq_u (matrix): Input equality constraint matrix (num_eq_input x nu) + % beq_u (vector): Input equality constraint values (num_eq_input x 1) + obj.check_setup(); + + % Handle empty inputs + if isempty(Aeq_x) + Aeq_x = zeros(0, obj.nx); + beq_x = zeros(0, 1); + end + if isempty(Aeq_u) + Aeq_u = zeros(0, obj.nu); + beq_u = zeros(0, 1); + end + + % Create inequality constraints: Ax <= b and -Ax <= -b (equivalent to Ax >= b) + % This gives us Ax == b + Alin_x = [Aeq_x; -Aeq_x]; + blin_x = [beq_x; -beq_x]; + + Alin_u = [Aeq_u; -Aeq_u]; + blin_u = [beq_u; -beq_u]; + + % Set the linear constraints + obj.set_linear_constraints(Alin_x, blin_x, Alin_u, blin_u); + fprintf('Equality constraints set: %d state equalities, %d input equalities\n', ... + size(Aeq_x, 1), size(Aeq_u, 1)); + end + function reset(obj) % Reset solver if obj.is_setup diff --git a/src/bindings.cpp b/src/bindings.cpp index d8f13c5..73120d4 100644 --- a/src/bindings.cpp +++ b/src/bindings.cpp @@ -437,6 +437,109 @@ void set_cache_terms(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) } } +// Set linear constraints (matches Python PyTinySolver::set_linear_constraints) +void set_linear_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { + if (nrhs != 5) { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "set_linear_constraints requires 5 input arguments"); + } + + if (!g_solver) { + mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); + } + + auto Alin_x = matlab_to_eigen(prhs[0]); + auto blin_x = matlab_to_eigen(prhs[1]); + auto Alin_u = matlab_to_eigen(prhs[2]); + auto blin_u = matlab_to_eigen(prhs[3]); + int verbose = (int)mxGetScalar(prhs[4]); + + try { + // Convert matrices to proper format for C++ API + tinyMatrix Alin_x_tiny = Alin_x.cast(); + tinyVector blin_x_tiny = blin_x.cast(); + tinyMatrix Alin_u_tiny = Alin_u.cast(); + tinyVector blin_u_tiny = blin_u.cast(); + + // Call C++ API function + int status = tiny_set_linear_constraints(g_solver.get(), Alin_x_tiny, blin_x_tiny, Alin_u_tiny, blin_u_tiny); + + if (status != 0) { + mexErrMsgIdAndTxt("TinyMPC:SetLinearConstraintsFailed", "tiny_set_linear_constraints failed with status %d", status); + } + + if (verbose) { + mexPrintf("Linear constraints set successfully\n"); + mexPrintf("State constraints: %dx%d matrix, Input constraints: %dx%d matrix\n", + (int)Alin_x.rows(), (int)Alin_x.cols(), (int)Alin_u.rows(), (int)Alin_u.cols()); + } + + } catch (const std::exception& e) { + mexErrMsgIdAndTxt("TinyMPC:SetLinearConstraintsException", + "Error setting linear constraints: %s", e.what()); + } +} + +// Helper function to convert MATLAB array to Eigen VectorXi +Eigen::VectorXi matlab_to_eigen_vectorxi(const mxArray* mx_array) { + if (!mxIsDouble(mx_array) || mxIsComplex(mx_array)) { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "Input must be a real double array"); + } + + size_t rows = mxGetM(mx_array); + size_t cols = mxGetN(mx_array); + double* data = mxGetPr(mx_array); + + // Convert to integer vector + Eigen::VectorXi result(rows * cols); + for (size_t i = 0; i < rows * cols; ++i) { + result(i) = (int)data[i]; + } + + return result; +} + +// Set conic constraints (inputs first, then states) +void set_cone_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { + if (nrhs != 7) { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "set_cone_constraints requires 7 input arguments"); + } + + if (!g_solver) { + mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); + } + + // Expected order now: Acu, qcu, cu, Acx, qcx, cx + auto Acu = matlab_to_eigen_vectorxi(prhs[0]); + auto qcu = matlab_to_eigen_vectorxi(prhs[1]); + auto cu = matlab_to_eigen(prhs[2]); + auto Acx = matlab_to_eigen_vectorxi(prhs[3]); + auto qcx = matlab_to_eigen_vectorxi(prhs[4]); + auto cx = matlab_to_eigen(prhs[5]); + int verbose = (int)mxGetScalar(prhs[6]); + + try { + // Convert matrices to proper format for C++ API + tinyVector cx_tiny = cx.cast(); + tinyVector cu_tiny = cu.cast(); + + // Call C++ API function (inputs first, states second) + int status = tiny_set_cone_constraints(g_solver.get(), Acu, qcu, cu_tiny, Acx, qcx, cx_tiny); + + if (status != 0) { + mexErrMsgIdAndTxt("TinyMPC:SetConeConstraintsFailed", "tiny_set_cone_constraints failed with status %d", status); + } + + if (verbose) { + mexPrintf("Cone constraints set successfully\n"); + mexPrintf("Input cones: %d, State cones: %d\n", (int)Acu.rows(), (int)Acx.rows()); + } + + } catch (const std::exception& e) { + mexErrMsgIdAndTxt("TinyMPC:SetConeConstraintsException", + "Error setting cone constraints: %s", e.what()); + } +} + // Code generation with sensitivity matrices (simplified - file operations moved to MATLAB) void codegen_with_sensitivity(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if (nrhs != 6) { @@ -629,6 +732,10 @@ void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { update_settings(nlhs, plhs, nrhs-1, prhs+1); } else if (strcmp(func_name, "print_problem_data") == 0) { print_problem_data(nlhs, plhs, nrhs-1, prhs+1); + } else if (strcmp(func_name, "set_linear_constraints") == 0) { + set_linear_constraints(nlhs, plhs, nrhs-1, prhs+1); + } else if (strcmp(func_name, "set_cone_constraints") == 0) { + set_cone_constraints(nlhs, plhs, nrhs-1, prhs+1); } else { mexErrMsgIdAndTxt("TinyMPC:InvalidFunction", "Unknown function: %s", func_name); } From 591c1046d9889934a02939c0a8d3af9ddd8641a4 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Wed, 20 Aug 2025 12:47:26 -0700 Subject: [PATCH 2/5] Linear, Cone, Equality constraints, tested with rocket example --- examples/rocket_landing_constraints.m | 22 +++--- src/TinyMPC.m | 107 +++----------------------- src/bindings.cpp | 106 +++++++------------------ 3 files changed, 46 insertions(+), 189 deletions(-) diff --git a/examples/rocket_landing_constraints.m b/examples/rocket_landing_constraints.m index 05dcffa..8132c9d 100644 --- a/examples/rocket_landing_constraints.m +++ b/examples/rocket_landing_constraints.m @@ -1,10 +1,7 @@ %% Rocket Landing with Constraints % Based on: https://github.com/TinyMPC/TinyMPC/blob/main/examples/rocket_landing_mpc.cpp -% MATLAB version of the Python rocket_landing_constraints.py example -clear; clc; close all; - -% Add TinyMPC class to path (following existing examples) +% Add TinyMPC class to path currentFile = mfilename('fullpath'); [scriptPath, ~, ~] = fileparts(currentFile); repoRoot = fileparts(scriptPath); @@ -44,8 +41,8 @@ % 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++) +Acx = [0]; % start indices for state cones +Acu = [0]; % start indices for input cones qcx = [3]; % dimensions for state cones qcu = [3]; % dimensions for input cones @@ -56,22 +53,21 @@ 'max_iter', 100, 'abs_pri_tol', 2e-3, 'verbose', true); % Set cone constraints -solver.set_cone_constraints(Acx, qcx, cx, Acu, qcu, cu); +solver.set_cone_constraints(Acu, qcu, cu, Acx, qcx, cx); -% Initial and goal states (matching corrected C++ version) +% 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]; xinit_scaled = xinit * 1.1; -% Initial reference trajectory (will be updated each step like C++) +% Initial reference trajectory x_ref = zeros(NSTATES, NHORIZON); u_ref = zeros(NINPUTS, NHORIZON-1); -% Animation setup - Extended for longer simulation NTOTAL = 100; % Match C++ x_current = xinit_scaled; % Match C++ (x0 = xinit * 1.1) -% Set initial reference (match C++ initial setup) +% Set initial reference % C++: work->Xref.col(i) = xinit + (xg - xinit)*tinytype(i)/(NTOTAL-1); % C++ i: 0 to NHORIZON-1, so MATLAB equivalent: (i-1) for i = 1:NHORIZON @@ -89,14 +85,14 @@ fprintf('Starting rocket landing simulation...\n'); for k = 1:(NTOTAL - NHORIZON) - % Calculate tracking error (match C++ exactly: (x0 - Xref.col(1)).norm()) + % Calculate tracking error tracking_error = norm(x_current - x_ref(:, 2)); % MATLAB is 1-indexed fprintf('tracking error: %.5f\n', tracking_error); % 1. Update measurement (set current state) solver.set_x0(x_current); - % 2. Update reference trajectory (match C++ exactly) + % 2. Update reference trajectory % C++: work->Xref.col(i) = xinit + (xg - xinit)*tinytype(i+k)/(NTOTAL-1); % C++ i: 0 to NHORIZON-1, k: 0 to NTOTAL-NHORIZON-1 % MATLAB i: 1 to NHORIZON, k: 1 to NTOTAL-NHORIZON diff --git a/src/TinyMPC.m b/src/TinyMPC.m index e94f598..87d1e8f 100644 --- a/src/TinyMPC.m +++ b/src/TinyMPC.m @@ -27,8 +27,8 @@ obj.settings.abs_dua_tol = 1e-4; obj.settings.max_iter = 100; obj.settings.check_termination = 1; - obj.settings.en_state_bound = false; - obj.settings.en_input_bound = false; + obj.settings.en_state_bound = true; + obj.settings.en_input_bound = true; obj.settings.adaptive_rho = false; obj.settings.adaptive_rho_min = 0.1; obj.settings.adaptive_rho_max = 10.0; @@ -54,7 +54,7 @@ function setup(obj, A, B, Q, R, N, varargin) opts = obj.parse_options(struct('rho', 1.0, 'fdyn', [], 'verbose', false, ... 'x_min', [], 'x_max', [], 'u_min', [], 'u_max', [], ... 'abs_pri_tol', 1e-4, 'abs_dua_tol', 1e-4, 'max_iter', 100, ... - 'check_termination', 1, 'en_state_bound', false, 'en_input_bound', false, ... + 'check_termination', 1, 'en_state_bound', true, 'en_input_bound', true, ... 'adaptive_rho', false, 'adaptive_rho_min', 0.1, 'adaptive_rho_max', 10.0, ... 'adaptive_rho_enable_clipping', true), varargin{:}); @@ -258,113 +258,24 @@ function set_sensitivity_matrices(obj, dK, dP, dC1, dC2) function set_linear_constraints(obj, Alin_x, blin_x, Alin_u, blin_u) % Set linear constraints: Alin_x * x <= blin_x, Alin_u * u <= blin_u - % - % Args: - % Alin_x (matrix): State constraint matrix (num_state_constraints x nx) - % blin_x (vector): State constraint bounds (num_state_constraints x 1) - % Alin_u (matrix): Input constraint matrix (num_input_constraints x nu) - % blin_u (vector): Input constraint bounds (num_input_constraints x 1) obj.check_setup(); - - % Validate dimensions - if ~isempty(Alin_x) - assert(size(Alin_x, 2) == obj.nx, 'Alin_x must have nx columns'); - assert(size(blin_x, 1) == size(Alin_x, 1), 'blin_x must match Alin_x rows'); - assert(size(blin_x, 2) == 1, 'blin_x must be a column vector'); - else - Alin_x = zeros(0, obj.nx); - blin_x = zeros(0, 1); - end - - if ~isempty(Alin_u) - assert(size(Alin_u, 2) == obj.nu, 'Alin_u must have nu columns'); - assert(size(blin_u, 1) == size(Alin_u, 1), 'blin_u must match Alin_u rows'); - assert(size(blin_u, 2) == 1, 'blin_u must be a column vector'); - else - Alin_u = zeros(0, obj.nu); - blin_u = zeros(0, 1); - end - - % Call MEX function tinympc_matlab('set_linear_constraints', Alin_x, blin_x, Alin_u, blin_u, false); - fprintf('Linear constraints set: %d state constraints, %d input constraints\n', ... - size(Alin_x, 1), size(Alin_u, 1)); end function set_cone_constraints(obj, Acu, qcu, cu, Acx, qcx, cx) % Set second-order cone constraints (inputs first, then states) - % - % Args: - % Acx (vector): Start indices for each state cone constraint (integer vector) - % qcx (vector): Dimension of each state cone constraint (integer vector) - % cx (vector): Cone parameter for each state cone constraint - % Acu (vector): Start indices for each input cone constraint (integer vector) - % qcu (vector): Dimension of each input cone constraint (integer vector) - % cu (vector): Cone parameter for each input cone constraint obj.check_setup(); - - % Validate dimensions - if ~isempty(Acu) - assert(length(qcu) == length(Acu), 'qcu must match Acu length'); - assert(length(cu) == length(Acu), 'cu must match Acu length'); - else - Acu = []; - qcu = []; - cu = []; - end - - if ~isempty(Acx) - assert(length(qcx) == length(Acx), 'qcx must match Acx length'); - assert(length(cx) == length(Acx), 'cx must match Acx length'); - else - Acx = []; - qcx = []; - cx = []; - end - - % Ensure column vectors - Acu = Acu(:); qcu = qcu(:); cu = cu(:); - Acx = Acx(:); qcx = qcx(:); cx = cx(:); - - % Call MEX function - tinympc_matlab('set_cone_constraints', Acx, qcx, cx, Acu, qcu, cu, false); % MEX expects Acx first by historical order - fprintf('Cone constraints set: %d input cones, %d state cones\n', ... - length(Acu), length(Acx)); + % Convert to proper types: indices to int32, parameters to double + if ~isempty(Acu), Acu = int32(Acu(:)); qcu = int32(qcu(:)); cu = double(cu(:)); end + if ~isempty(Acx), Acx = int32(Acx(:)); qcx = int32(qcx(:)); cx = double(cx(:)); end + tinympc_matlab('set_cone_constraints', Acu, qcu, cu, Acx, qcx, cx, false); end function set_equality_constraints(obj, Aeq_x, beq_x, Aeq_u, beq_u) % Set equality constraints: Aeq_x * x == beq_x, Aeq_u * u == beq_u - % Implemented as two inequalities: <= and >= - % - % Args: - % Aeq_x (matrix): State equality constraint matrix (num_eq_state x nx) - % beq_x (vector): State equality constraint values (num_eq_state x 1) - % Aeq_u (matrix): Input equality constraint matrix (num_eq_input x nu) - % beq_u (vector): Input equality constraint values (num_eq_input x 1) + % Implemented as two inequalities: Ax <= b and -Ax <= -b obj.check_setup(); - - % Handle empty inputs - if isempty(Aeq_x) - Aeq_x = zeros(0, obj.nx); - beq_x = zeros(0, 1); - end - if isempty(Aeq_u) - Aeq_u = zeros(0, obj.nu); - beq_u = zeros(0, 1); - end - - % Create inequality constraints: Ax <= b and -Ax <= -b (equivalent to Ax >= b) - % This gives us Ax == b - Alin_x = [Aeq_x; -Aeq_x]; - blin_x = [beq_x; -beq_x]; - - Alin_u = [Aeq_u; -Aeq_u]; - blin_u = [beq_u; -beq_u]; - - % Set the linear constraints - obj.set_linear_constraints(Alin_x, blin_x, Alin_u, blin_u); - fprintf('Equality constraints set: %d state equalities, %d input equalities\n', ... - size(Aeq_x, 1), size(Aeq_u, 1)); + obj.set_linear_constraints([Aeq_x; -Aeq_x], [beq_x; -beq_x], [Aeq_u; -Aeq_u], [beq_u; -beq_u]); end function reset(obj) diff --git a/src/bindings.cpp b/src/bindings.cpp index 73120d4..8778173 100644 --- a/src/bindings.cpp +++ b/src/bindings.cpp @@ -437,106 +437,56 @@ void set_cache_terms(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) } } -// Set linear constraints (matches Python PyTinySolver::set_linear_constraints) +// Set linear constraints void set_linear_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { - if (nrhs != 5) { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "set_linear_constraints requires 5 input arguments"); - } - - if (!g_solver) { - mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); + if (nrhs != 5 || !g_solver) { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "Invalid arguments or solver not initialized"); } - auto Alin_x = matlab_to_eigen(prhs[0]); - auto blin_x = matlab_to_eigen(prhs[1]); - auto Alin_u = matlab_to_eigen(prhs[2]); - auto blin_u = matlab_to_eigen(prhs[3]); - int verbose = (int)mxGetScalar(prhs[4]); + auto Alin_x = matlab_to_eigen(prhs[0]).cast(); + auto blin_x = matlab_to_eigen(prhs[1]).cast(); + auto Alin_u = matlab_to_eigen(prhs[2]).cast(); + auto blin_u = matlab_to_eigen(prhs[3]).cast(); - try { - // Convert matrices to proper format for C++ API - tinyMatrix Alin_x_tiny = Alin_x.cast(); - tinyVector blin_x_tiny = blin_x.cast(); - tinyMatrix Alin_u_tiny = Alin_u.cast(); - tinyVector blin_u_tiny = blin_u.cast(); - - // Call C++ API function - int status = tiny_set_linear_constraints(g_solver.get(), Alin_x_tiny, blin_x_tiny, Alin_u_tiny, blin_u_tiny); - - if (status != 0) { - mexErrMsgIdAndTxt("TinyMPC:SetLinearConstraintsFailed", "tiny_set_linear_constraints failed with status %d", status); - } - - if (verbose) { - mexPrintf("Linear constraints set successfully\n"); - mexPrintf("State constraints: %dx%d matrix, Input constraints: %dx%d matrix\n", - (int)Alin_x.rows(), (int)Alin_x.cols(), (int)Alin_u.rows(), (int)Alin_u.cols()); - } - - } catch (const std::exception& e) { - mexErrMsgIdAndTxt("TinyMPC:SetLinearConstraintsException", - "Error setting linear constraints: %s", e.what()); + int status = tiny_set_linear_constraints(g_solver.get(), Alin_x, blin_x, Alin_u, blin_u); + if (status != 0) { + mexErrMsgIdAndTxt("TinyMPC:SetLinearConstraintsFailed", "Failed with status %d", status); } } // Helper function to convert MATLAB array to Eigen VectorXi Eigen::VectorXi matlab_to_eigen_vectorxi(const mxArray* mx_array) { - if (!mxIsDouble(mx_array) || mxIsComplex(mx_array)) { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "Input must be a real double array"); + size_t size = mxGetM(mx_array) * mxGetN(mx_array); + Eigen::VectorXi result(size); + + if (mxIsInt32(mx_array)) { + int32_t* data = (int32_t*)mxGetData(mx_array); + for (size_t i = 0; i < size; ++i) result(i) = data[i]; + } else if (mxIsDouble(mx_array)) { + double* data = mxGetPr(mx_array); + for (size_t i = 0; i < size; ++i) result(i) = (int)round(data[i]); + } else { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "Input must be int32 or double array"); } - - size_t rows = mxGetM(mx_array); - size_t cols = mxGetN(mx_array); - double* data = mxGetPr(mx_array); - - // Convert to integer vector - Eigen::VectorXi result(rows * cols); - for (size_t i = 0; i < rows * cols; ++i) { - result(i) = (int)data[i]; - } - return result; } // Set conic constraints (inputs first, then states) void set_cone_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { - if (nrhs != 7) { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "set_cone_constraints requires 7 input arguments"); + if (nrhs != 7 || !g_solver) { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "Invalid arguments or solver not initialized"); } - if (!g_solver) { - mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); - } - - // Expected order now: Acu, qcu, cu, Acx, qcx, cx auto Acu = matlab_to_eigen_vectorxi(prhs[0]); auto qcu = matlab_to_eigen_vectorxi(prhs[1]); - auto cu = matlab_to_eigen(prhs[2]); + auto cu = matlab_to_eigen(prhs[2]).cast(); auto Acx = matlab_to_eigen_vectorxi(prhs[3]); auto qcx = matlab_to_eigen_vectorxi(prhs[4]); - auto cx = matlab_to_eigen(prhs[5]); - int verbose = (int)mxGetScalar(prhs[6]); + auto cx = matlab_to_eigen(prhs[5]).cast(); - try { - // Convert matrices to proper format for C++ API - tinyVector cx_tiny = cx.cast(); - tinyVector cu_tiny = cu.cast(); - - // Call C++ API function (inputs first, states second) - int status = tiny_set_cone_constraints(g_solver.get(), Acu, qcu, cu_tiny, Acx, qcx, cx_tiny); - - if (status != 0) { - mexErrMsgIdAndTxt("TinyMPC:SetConeConstraintsFailed", "tiny_set_cone_constraints failed with status %d", status); - } - - if (verbose) { - mexPrintf("Cone constraints set successfully\n"); - mexPrintf("Input cones: %d, State cones: %d\n", (int)Acu.rows(), (int)Acx.rows()); - } - - } catch (const std::exception& e) { - mexErrMsgIdAndTxt("TinyMPC:SetConeConstraintsException", - "Error setting cone constraints: %s", e.what()); + int status = tiny_set_cone_constraints(g_solver.get(), Acu, qcu, cu, Acx, qcx, cx); + if (status != 0) { + mexErrMsgIdAndTxt("TinyMPC:SetConeConstraintsFailed", "Failed with status %d", status); } } From a315bfe6c6cef1becc99905fecfe48bc40848ae8 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Wed, 20 Aug 2025 16:11:52 -0700 Subject: [PATCH 3/5] Constraints working, bit messy --- README.md | 26 ++- examples/cartpole_example_code_generation.m | 7 +- ...rtpole_example_mpc_reference_constrained.m | 7 +- examples/interactive_cartpole.m | 3 +- examples/rocket_landing_constraints.m | 15 +- src/TinyMPC.m | 94 ++++++++--- src/bindings.cpp | 152 ++++++++++++------ 7 files changed, 218 insertions(+), 86 deletions(-) diff --git a/README.md b/README.md index 6025c23..66367f2 100644 --- a/README.md +++ b/README.md @@ -88,10 +88,13 @@ controls_trajectory = solution.controls; % All predicted controls (1×19) ### Code Generation Workflow ```matlab -% Setup solver with constraints +% Setup solver solver = TinyMPC(); u_min = -0.5; u_max = 0.5; % Control bounds -solver.setup(A, B, Q, R, N, 'u_min', u_min, 'u_max', u_max, 'rho', 1.0); +solver.setup(A, B, Q, R, N, 'rho', 1.0); + +% Set bounds explicitly +solver.set_bound_constraints([], [], u_min, u_max); % Generate C++ code solver.codegen('out'); @@ -148,6 +151,25 @@ solver.codegen_with_sensitivity(output_dir, dK, dP, dC1, dC2) [dK, dP, dC1, dC2] = solver.compute_sensitivity_autograd() ``` +### Constraints API + +```matlab +% Bounds (box constraints) +solver.set_bound_constraints(x_min, x_max, u_min, u_max); + +% Linear inequalities +solver.set_linear_constraints(Alin_x, blin_x, Alin_u, blin_u); + +% Second-order cones (inputs first, then states) +solver.set_cone_constraints(Acu, qcu, cu, Acx, qcx, cx); + +% Equality constraints (implemented as paired inequalities) +% Aeq_x * x == beq_x, Aeq_u * u == beq_u +solver.set_equality_constraints(Aeq_x, beq_x, Aeq_u, beq_u); +``` + +Each call auto-enables the corresponding constraint(s) in the C++ layer. + ### Configuration ```matlab diff --git a/examples/cartpole_example_code_generation.m b/examples/cartpole_example_code_generation.m index e840d0c..410e810 100644 --- a/examples/cartpole_example_code_generation.m +++ b/examples/cartpole_example_code_generation.m @@ -26,8 +26,11 @@ % Create solver solver = TinyMPC(); -% Setup solver with matrices and constraints -solver.setup(A, B, Q, R, N, 'u_min', u_min, 'u_max', u_max, 'rho', 1.0, 'verbose', true); +% Setup solver +solver.setup(A, B, Q, R, N, 'rho', 1.0, 'verbose', true); + +% Set bounds explicitly +solver.set_bound_constraints([], [], u_min, u_max); % Generate code solver.codegen('out'); diff --git a/examples/cartpole_example_mpc_reference_constrained.m b/examples/cartpole_example_mpc_reference_constrained.m index a80dc79..9773400 100644 --- a/examples/cartpole_example_mpc_reference_constrained.m +++ b/examples/cartpole_example_mpc_reference_constrained.m @@ -26,8 +26,11 @@ % Create solver solver = TinyMPC(); -% Setup solver with matrices and constraints -solver.setup(A, B, Q, R, N, 'u_min', u_min, 'u_max', u_max); +% Setup solver +solver.setup(A, B, Q, R, N); + +% Set bounds explicitly +solver.set_bound_constraints([], [], u_min, u_max); % Set reference trajectory (goal must be another equilibrium position) x_ref = repmat([1.0; 0; 0; 0], 1, N); % 4x20 matrix diff --git a/examples/interactive_cartpole.m b/examples/interactive_cartpole.m index da8c90e..34d781b 100644 --- a/examples/interactive_cartpole.m +++ b/examples/interactive_cartpole.m @@ -38,7 +38,8 @@ u_min = -5; % Force constraint (min) u_max = 5; % Force constraint (max) -prob.setup(A, B, Q, R, N, 'u_min', u_min, 'u_max', u_max, 'rho', 0.1); +prob.setup(A, B, Q, R, N, 'rho', 0.1); +prob.set_bound_constraints([], [], u_min, u_max); % Set reference trajectory (origin stabilization) Xref = zeros(n, N); % nx x N diff --git a/examples/rocket_landing_constraints.m b/examples/rocket_landing_constraints.m index 8132c9d..993a3e5 100644 --- a/examples/rocket_landing_constraints.m +++ b/examples/rocket_landing_constraints.m @@ -49,9 +49,10 @@ % Setup solver solver = TinyMPC(); solver.setup(A, B, Q, R, NHORIZON, 'rho', 1.0, 'fdyn', fdyn, ... - 'x_min', x_min, 'x_max', x_max, 'u_min', u_min, 'u_max', u_max, ... 'max_iter', 100, 'abs_pri_tol', 2e-3, 'verbose', true); +solver.set_bound_constraints(x_min, x_max, u_min, u_max); + % Set cone constraints solver.set_cone_constraints(Acu, qcu, cu, Acx, qcx, cx); @@ -64,14 +65,12 @@ x_ref = zeros(NSTATES, NHORIZON); u_ref = zeros(NINPUTS, NHORIZON-1); -NTOTAL = 100; % Match C++ -x_current = xinit_scaled; % Match C++ (x0 = xinit * 1.1) +NTOTAL = 100; +x_current = xinit_scaled; % Set initial reference -% C++: work->Xref.col(i) = xinit + (xg - xinit)*tinytype(i)/(NTOTAL-1); -% C++ i: 0 to NHORIZON-1, so MATLAB equivalent: (i-1) for i = 1:NHORIZON - x_ref(:, i) = xinit + (xgoal - xinit) * (i-1) / (NTOTAL - 1); % Use NTOTAL-1 like C++ + x_ref(:, i) = xinit + (xgoal - xinit) * (i-1) / (NTOTAL - 1); end u_ref(3, :) = 10.0; % Hover thrust @@ -93,10 +92,6 @@ solver.set_x0(x_current); % 2. Update reference trajectory - % C++: work->Xref.col(i) = xinit + (xg - xinit)*tinytype(i+k)/(NTOTAL-1); - % C++ i: 0 to NHORIZON-1, k: 0 to NTOTAL-NHORIZON-1 - % MATLAB i: 1 to NHORIZON, k: 1 to NTOTAL-NHORIZON - % So MATLAB equivalent: (i-1)+(k-1) = i+k-2 for i = 1:NHORIZON x_ref(:, i) = xinit + (xgoal - xinit) * (i + k - 2) / (NTOTAL - 1); if i <= NHORIZON - 1 diff --git a/src/TinyMPC.m b/src/TinyMPC.m index 87d1e8f..ca2a46c 100644 --- a/src/TinyMPC.m +++ b/src/TinyMPC.m @@ -27,8 +27,12 @@ obj.settings.abs_dua_tol = 1e-4; obj.settings.max_iter = 100; obj.settings.check_termination = 1; - obj.settings.en_state_bound = true; - obj.settings.en_input_bound = true; + obj.settings.en_state_bound = false; + obj.settings.en_input_bound = false; + obj.settings.en_state_soc = false; + obj.settings.en_input_soc = false; + obj.settings.en_state_linear = false; + obj.settings.en_input_linear = false; obj.settings.adaptive_rho = false; obj.settings.adaptive_rho_min = 0.1; obj.settings.adaptive_rho_max = 10.0; @@ -36,7 +40,7 @@ end function setup(obj, A, B, Q, R, N, varargin) - % Setup the TinyMPC solver + % Setup the TinyMPC solver % Usage: obj.setup(A, B, Q, R, N, 'rho', 1.5, 'verbose', true) % Basic dimension validation @@ -52,9 +56,8 @@ function setup(obj, A, B, Q, R, N, varargin) % Parse options using name-value pairs opts = obj.parse_options(struct('rho', 1.0, 'fdyn', [], 'verbose', false, ... - 'x_min', [], 'x_max', [], 'u_min', [], 'u_max', [], ... 'abs_pri_tol', 1e-4, 'abs_dua_tol', 1e-4, 'max_iter', 100, ... - 'check_termination', 1, 'en_state_bound', true, 'en_input_bound', true, ... + 'check_termination', 1, 'en_state_bound', false, 'en_input_bound', false, ... 'adaptive_rho', false, 'adaptive_rho_min', 0.1, 'adaptive_rho_max', 10.0, ... 'adaptive_rho_enable_clipping', true), varargin{:}); @@ -65,8 +68,9 @@ function setup(obj, A, B, Q, R, N, varargin) obj.settings.abs_dua_tol = opts.abs_dua_tol; obj.settings.max_iter = opts.max_iter; obj.settings.check_termination = opts.check_termination; - obj.settings.en_state_bound = opts.en_state_bound; - obj.settings.en_input_bound = opts.en_input_bound; + % Do not enable bound constraints in setup; only via set_bound_constraints + obj.settings.en_state_bound = false; + obj.settings.en_input_bound = false; obj.settings.adaptive_rho = opts.adaptive_rho; obj.settings.adaptive_rho_min = opts.adaptive_rho_min; obj.settings.adaptive_rho_max = opts.adaptive_rho_max; @@ -79,15 +83,9 @@ function setup(obj, A, B, Q, R, N, varargin) fdyn = opts.fdyn; end - % Process bounds with simple expansion - obj.x_min = obj.expand_bounds(opts.x_min, obj.nx, obj.N, -1e17); - obj.x_max = obj.expand_bounds(opts.x_max, obj.nx, obj.N, +1e17); - obj.u_min = obj.expand_bounds(opts.u_min, obj.nu, obj.N-1, -1e17); - obj.u_max = obj.expand_bounds(opts.u_max, obj.nu, obj.N-1, +1e17); - % Call MEX setup function status = tinympc_matlab('setup', obj.A, obj.B, fdyn, obj.Q, obj.R, ... - obj.rho, obj.nx, obj.nu, obj.N, obj.x_min, obj.x_max, obj.u_min, obj.u_max, opts.verbose); + obj.rho, obj.nx, obj.nu, obj.N, opts.verbose); if status == 0 obj.is_setup = true; @@ -95,8 +93,9 @@ function setup(obj, A, B, Q, R, N, varargin) % Push settings to C++ layer (including adaptive_rho settings) tinympc_matlab('update_settings', obj.settings.abs_pri_tol, obj.settings.abs_dua_tol, ... obj.settings.max_iter, obj.settings.check_termination, obj.settings.en_state_bound, ... - obj.settings.en_input_bound, obj.settings.adaptive_rho, obj.settings.adaptive_rho_min, ... - obj.settings.adaptive_rho_max, obj.settings.adaptive_rho_enable_clipping, false); + obj.settings.en_input_bound, obj.settings.en_state_soc, obj.settings.en_input_soc, ... + obj.settings.en_state_linear, obj.settings.en_input_linear, obj.settings.adaptive_rho, ... + obj.settings.adaptive_rho_min, obj.settings.adaptive_rho_max, obj.settings.adaptive_rho_enable_clipping, false); if opts.verbose, fprintf('TinyMPC solver setup successful (nx=%d, nu=%d, N=%d)\n', obj.nx, obj.nu, obj.N); end else @@ -150,8 +149,9 @@ function update_settings(obj, varargin) end tinympc_matlab('update_settings', obj.settings.abs_pri_tol, obj.settings.abs_dua_tol, ... obj.settings.max_iter, obj.settings.check_termination, obj.settings.en_state_bound, ... - obj.settings.en_input_bound, obj.settings.adaptive_rho, obj.settings.adaptive_rho_min, ... - obj.settings.adaptive_rho_max, obj.settings.adaptive_rho_enable_clipping, false); + obj.settings.en_input_bound, obj.settings.en_state_soc, obj.settings.en_input_soc, ... + obj.settings.en_state_linear, obj.settings.en_input_linear, obj.settings.adaptive_rho, ... + obj.settings.adaptive_rho_min, obj.settings.adaptive_rho_max, obj.settings.adaptive_rho_enable_clipping, false); end function status = solve(obj) @@ -260,6 +260,37 @@ function set_linear_constraints(obj, Alin_x, blin_x, Alin_u, blin_u) % Set linear constraints: Alin_x * x <= blin_x, Alin_u * u <= blin_u obj.check_setup(); tinympc_matlab('set_linear_constraints', Alin_x, blin_x, Alin_u, blin_u, false); + + % Auto-enable linear constraints after successful setting + obj.settings.en_state_linear = ~isempty(Alin_x) && ~isempty(blin_x); + obj.settings.en_input_linear = ~isempty(Alin_u) && ~isempty(blin_u); + if obj.settings.en_state_linear || obj.settings.en_input_linear + obj.update_settings(); % Push to C++ layer + end + end + + function set_bound_constraints(obj, x_min, x_max, u_min, u_max) + % Set box constraints: x_min <= x <= x_max, u_min <= u <= u_max + obj.check_setup(); + + % Process bounds with simple expansion + x_min_expanded = obj.expand_bounds(x_min, obj.nx, obj.N, -1e17); + x_max_expanded = obj.expand_bounds(x_max, obj.nx, obj.N, +1e17); + u_min_expanded = obj.expand_bounds(u_min, obj.nu, obj.N-1, -1e17); + u_max_expanded = obj.expand_bounds(u_max, obj.nu, obj.N-1, +1e17); + + % Update stored bounds + obj.x_min = x_min_expanded; + obj.x_max = x_max_expanded; + obj.u_min = u_min_expanded; + obj.u_max = u_max_expanded; + + tinympc_matlab('set_bound_constraints', x_min_expanded, x_max_expanded, u_min_expanded, u_max_expanded, false); + + % Auto-enable bound constraints after successful setting + obj.settings.en_state_bound = true; + obj.settings.en_input_bound = true; + obj.update_settings(); % Push to C++ layer end function set_cone_constraints(obj, Acu, qcu, cu, Acx, qcx, cx) @@ -269,13 +300,36 @@ function set_cone_constraints(obj, Acu, qcu, cu, Acx, qcx, cx) if ~isempty(Acu), Acu = int32(Acu(:)); qcu = int32(qcu(:)); cu = double(cu(:)); end if ~isempty(Acx), Acx = int32(Acx(:)); qcx = int32(qcx(:)); cx = double(cx(:)); end tinympc_matlab('set_cone_constraints', Acu, qcu, cu, Acx, qcx, cx, false); + + % Auto-enable cone constraints after successful setting + obj.settings.en_state_soc = ~isempty(Acx) && ~isempty(qcx) && ~isempty(cx); + obj.settings.en_input_soc = ~isempty(Acu) && ~isempty(qcu) && ~isempty(cu); + if obj.settings.en_state_soc || obj.settings.en_input_soc + obj.update_settings(); % Push to C++ layer + end end function set_equality_constraints(obj, Aeq_x, beq_x, Aeq_u, beq_u) % Set equality constraints: Aeq_x * x == beq_x, Aeq_u * u == beq_u - % Implemented as two inequalities: Ax <= b and -Ax <= -b + % Implemented internally as paired inequalities in C++ obj.check_setup(); - obj.set_linear_constraints([Aeq_x; -Aeq_x], [beq_x; -beq_x], [Aeq_u; -Aeq_u], [beq_u; -beq_u]); + + % Normalize beq inputs to column vectors for safety + if ~isempty(beq_x) && isrow(beq_x), beq_x = beq_x'; end + if ~isempty(beq_u) && isrow(beq_u), beq_u = beq_u'; end + + % Convert equalities to two inequalities and delegate to linear API + Alin_x = []; blin_x = []; + if ~isempty(Aeq_x) + Alin_x = [Aeq_x; -Aeq_x]; + blin_x = [beq_x; -beq_x]; + end + Alin_u = []; blin_u = []; + if ~isempty(Aeq_u) + Alin_u = [Aeq_u; -Aeq_u]; + blin_u = [beq_u; -beq_u]; + end + obj.set_linear_constraints(Alin_x, blin_x, Alin_u, blin_u); end function reset(obj) diff --git a/src/bindings.cpp b/src/bindings.cpp index 8778173..0bb9aa4 100644 --- a/src/bindings.cpp +++ b/src/bindings.cpp @@ -43,18 +43,13 @@ mxArray* eigen_to_matlab(const Eigen::MatrixXd& eigen_mat) { return mx_array; } -// Setup function - initialize the solver (matches Python PyTinySolver constructor) +// Setup function - initialize the solver void setup_solver(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { - // Expected arguments: A, B, fdyn, Q, R, rho, nx, nu, N, x_min, x_max, u_min, u_max, verbose - if (nrhs != 14) { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "Setup requires 14 input arguments"); + // Expected arguments: A, B, fdyn, Q, R, rho, nx, nu, N, verbose + if (nrhs != 10) { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "setup requires 10 input arguments: A, B, fdyn, Q, R, rho, nx, nu, N, verbose"); } - // Extract problem dimensions - int nx = (int)mxGetScalar(prhs[6]); - int nu = (int)mxGetScalar(prhs[7]); - int N = (int)mxGetScalar(prhs[8]); - // Extract matrices auto A = matlab_to_eigen(prhs[0]); auto B = matlab_to_eigen(prhs[1]); @@ -63,13 +58,12 @@ void setup_solver(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { auto R = matlab_to_eigen(prhs[4]); double rho = mxGetScalar(prhs[5]); - // Extract bound matrices - auto x_min = matlab_to_eigen(prhs[9]); - auto x_max = matlab_to_eigen(prhs[10]); - auto u_min = matlab_to_eigen(prhs[11]); - auto u_max = matlab_to_eigen(prhs[12]); + // Extract problem dimensions + int nx = (int)mxGetScalar(prhs[6]); + int nu = (int)mxGetScalar(prhs[7]); + int N = (int)mxGetScalar(prhs[8]); - int verbose = (int)mxGetScalar(prhs[13]); + int verbose = (int)mxGetScalar(prhs[9]); if (verbose) { mexPrintf("Setting up TinyMPC solver with nx=%d, nu=%d, N=%d, rho=%f\n", nx, nu, N, rho); @@ -86,7 +80,7 @@ void setup_solver(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { tinyMatrix Q_tiny = Q.cast(); tinyMatrix R_tiny = R.cast(); - // Setup solver (exactly like Python PyTinySolver constructor) + // Setup solver int status = tiny_setup(&solver_ptr, A_tiny, B_tiny, fdyn_tiny, Q_tiny, R_tiny, (tinytype)rho, nx, nu, N, verbose); @@ -94,24 +88,6 @@ void setup_solver(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { mexErrMsgIdAndTxt("TinyMPC:SetupFailed", "tiny_setup failed with status %d", status); } - // Set bounds (exactly like Python PyTinySolver constructor) - tinyMatrix x_min_tiny = x_min.cast(); - tinyMatrix x_max_tiny = x_max.cast(); - tinyMatrix u_min_tiny = u_min.cast(); - tinyMatrix u_max_tiny = u_max.cast(); - - if (status == 0) { - status = tiny_set_bound_constraints(solver_ptr, x_min_tiny, x_max_tiny, u_min_tiny, u_max_tiny); - } - - if (status != 0) { - if (solver_ptr) { - // Clean up if needed - delete solver_ptr; - } - mexErrMsgIdAndTxt("TinyMPC:SetupFailed", "Bound constraints setup failed with status %d", status); - } - // Store solver (transfer ownership) g_solver.reset(solver_ptr); @@ -208,7 +184,7 @@ void set_u_ref(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { } } -// Set bound constraints (matches Python set_bound_constraints) +// Set bound constraints void set_bound_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if (nrhs != 5) { mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "set_bound_constraints requires 5 input arguments"); @@ -229,13 +205,17 @@ void set_bound_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* p tinyMatrix u_min_tiny = u_min.cast(); tinyMatrix u_max_tiny = u_max.cast(); - // Use the API function (same as Python) + // Use the API function int status = tiny_set_bound_constraints(g_solver.get(), x_min_tiny, x_max_tiny, u_min_tiny, u_max_tiny); if (status != 0) { mexErrMsgIdAndTxt("TinyMPC:SetBoundConstraintsFailed", "tiny_set_bound_constraints failed with status %d", status); } + // Auto-enable bound constraints after successful setting + g_solver->settings->en_state_bound = 1; + g_solver->settings->en_input_bound = 1; + if (verbose) { mexPrintf("Bound constraints set\n"); } @@ -444,16 +424,48 @@ void set_linear_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* } auto Alin_x = matlab_to_eigen(prhs[0]).cast(); - auto blin_x = matlab_to_eigen(prhs[1]).cast(); + Eigen::MatrixXd blin_x_in = matlab_to_eigen(prhs[1]); auto Alin_u = matlab_to_eigen(prhs[2]).cast(); - auto blin_u = matlab_to_eigen(prhs[3]).cast(); + Eigen::MatrixXd blin_u_in = matlab_to_eigen(prhs[3]); + + // Normalize b vectors to column vectors (Eigen's tinyVector expectation) + Eigen::Matrix blin_x; + if (blin_x_in.size() == 0) { + blin_x.resize(0); + } else if (blin_x_in.cols() == 1) { + blin_x = blin_x_in.cast(); + } else if (blin_x_in.rows() == 1) { + blin_x = blin_x_in.transpose().cast(); + } else { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "blin_x must be a vector (Nx1 or 1xN)"); + } + + Eigen::Matrix blin_u; + if (blin_u_in.size() == 0) { + blin_u.resize(0); + } else if (blin_u_in.cols() == 1) { + blin_u = blin_u_in.cast(); + } else if (blin_u_in.rows() == 1) { + blin_u = blin_u_in.transpose().cast(); + } else { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "blin_u must be a vector (Mx1 or 1xM)"); + } int status = tiny_set_linear_constraints(g_solver.get(), Alin_x, blin_x, Alin_u, blin_u); if (status != 0) { mexErrMsgIdAndTxt("TinyMPC:SetLinearConstraintsFailed", "Failed with status %d", status); } + + // Auto-enable linear constraints after successful setting + bool has_state_linear = (Alin_x.rows() > 0 && blin_x.rows() > 0); + bool has_input_linear = (Alin_u.rows() > 0 && blin_u.rows() > 0); + if (has_state_linear) { + g_solver->settings->en_state_linear = 1; + } + if (has_input_linear) { + g_solver->settings->en_input_linear = 1; + } } - // Helper function to convert MATLAB array to Eigen VectorXi Eigen::VectorXi matlab_to_eigen_vectorxi(const mxArray* mx_array) { size_t size = mxGetM(mx_array) * mxGetN(mx_array); @@ -479,15 +491,49 @@ void set_cone_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* pr auto Acu = matlab_to_eigen_vectorxi(prhs[0]); auto qcu = matlab_to_eigen_vectorxi(prhs[1]); - auto cu = matlab_to_eigen(prhs[2]).cast(); + Eigen::MatrixXd cu_in = matlab_to_eigen(prhs[2]); auto Acx = matlab_to_eigen_vectorxi(prhs[3]); auto qcx = matlab_to_eigen_vectorxi(prhs[4]); - auto cx = matlab_to_eigen(prhs[5]).cast(); + Eigen::MatrixXd cx_in = matlab_to_eigen(prhs[5]); + + // Normalize cu/cx to column vectors (0-length is allowed) + Eigen::VectorXd cu_vec; + if (cu_in.size() == 0) { + cu_vec.resize(0); + } else if (cu_in.cols() == 1) { + cu_vec = cu_in.col(0); + } else if (cu_in.rows() == 1) { + cu_vec = cu_in.transpose().col(0); + } else { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "cu must be a vector (Kx1 or 1xK)"); + } + + Eigen::VectorXd cx_vec; + if (cx_in.size() == 0) { + cx_vec.resize(0); + } else if (cx_in.cols() == 1) { + cx_vec = cx_in.col(0); + } else if (cx_in.rows() == 1) { + cx_vec = cx_in.transpose().col(0); + } else { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "cx must be a vector (Kx1 or 1xK)"); + } - int status = tiny_set_cone_constraints(g_solver.get(), Acu, qcu, cu, Acx, qcx, cx); + int status = tiny_set_cone_constraints(g_solver.get(), Acu, qcu, cu_vec.cast(), + Acx, qcx, cx_vec.cast()); if (status != 0) { mexErrMsgIdAndTxt("TinyMPC:SetConeConstraintsFailed", "Failed with status %d", status); } + + // Auto-enable cone constraints after successful setting + bool has_state_cones = (Acx.size() > 0 && qcx.size() > 0 && cx_vec.size() > 0); + bool has_input_cones = (Acu.size() > 0 && qcu.size() > 0 && cu_vec.size() > 0); + if (has_state_cones) { + g_solver->settings->en_state_soc = 1; + } + if (has_input_cones) { + g_solver->settings->en_input_soc = 1; + } } // Code generation with sensitivity matrices (simplified - file operations moved to MATLAB) @@ -559,8 +605,8 @@ void reset_solver(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { // Update settings (matches Python PyTinySolver::update_settings) void update_settings(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { - if (nrhs != 11) { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "update_settings requires 11 input arguments"); + if (nrhs != 15) { + mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "update_settings requires 15 input arguments"); } if (!g_solver) { @@ -574,11 +620,15 @@ void update_settings(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) int check_termination = (int)mxGetScalar(prhs[3]); int en_state_bound = (int)mxGetScalar(prhs[4]); int en_input_bound = (int)mxGetScalar(prhs[5]); - int adaptive_rho = (int)mxGetScalar(prhs[6]); - double adaptive_rho_min = mxGetScalar(prhs[7]); - double adaptive_rho_max = mxGetScalar(prhs[8]); - int adaptive_rho_enable_clipping = (int)mxGetScalar(prhs[9]); - int verbose = (int)mxGetScalar(prhs[10]); + int en_state_soc = (int)mxGetScalar(prhs[6]); + int en_input_soc = (int)mxGetScalar(prhs[7]); + int en_state_linear = (int)mxGetScalar(prhs[8]); + int en_input_linear = (int)mxGetScalar(prhs[9]); + int adaptive_rho = (int)mxGetScalar(prhs[10]); + double adaptive_rho_min = mxGetScalar(prhs[11]); + double adaptive_rho_max = mxGetScalar(prhs[12]); + int adaptive_rho_enable_clipping = (int)mxGetScalar(prhs[13]); + int verbose = (int)mxGetScalar(prhs[14]); try { if (g_solver && g_solver->settings) { @@ -589,6 +639,10 @@ void update_settings(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) 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; // Copy adaptive rho settings g_solver->settings->adaptive_rho = adaptive_rho; From 6b7970b8eb0fa56680cd445a93572edfd7f5d2c1 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Wed, 20 Aug 2025 16:20:46 -0700 Subject: [PATCH 4/5] Cone, Linear, Equality, Bound Constraints implemented --- src/TinyMPC.m | 28 ++++----------- src/bindings.cpp | 91 ++++++++---------------------------------------- 2 files changed, 21 insertions(+), 98 deletions(-) diff --git a/src/TinyMPC.m b/src/TinyMPC.m index ca2a46c..d566216 100644 --- a/src/TinyMPC.m +++ b/src/TinyMPC.m @@ -109,11 +109,6 @@ function set_x0(obj, x0) tinympc_matlab('set_x0', x0(:), false); end - function set_initial_state(obj, x0) - % Alias for set_x0 (compatibility) - obj.set_x0(x0); - end - function set_x_ref(obj, x_ref) % Set state reference trajectory obj.check_setup(); @@ -121,11 +116,6 @@ function set_x_ref(obj, x_ref) tinympc_matlab('set_x_ref', x_ref_expanded, false); end - function set_state_reference(obj, x_ref) - % Alias for set_x_ref (compatibility) - obj.set_x_ref(x_ref); - end - function set_u_ref(obj, u_ref) % Set input reference trajectory obj.check_setup(); @@ -133,14 +123,8 @@ function set_u_ref(obj, u_ref) tinympc_matlab('set_u_ref', u_ref_expanded, false); end - function set_input_reference(obj, u_ref) - % Alias for set_u_ref (compatibility) - obj.set_u_ref(u_ref); - end - function update_settings(obj, varargin) - % Update solver settings - % Usage: obj.update_settings('max_iter', 200, 'abs_pri_tol', 1e-5) + % Update solver settings (flags and tolerances) obj.check_setup(); for i = 1:2:length(varargin) if isfield(obj.settings, varargin{i}) @@ -257,7 +241,7 @@ function set_sensitivity_matrices(obj, dK, dP, dC1, dC2) end function set_linear_constraints(obj, Alin_x, blin_x, Alin_u, blin_u) - % Set linear constraints: Alin_x * x <= blin_x, Alin_u * u <= blin_u + % Linear constraints: Alin_x * x <= blin_x, Alin_u * u <= blin_u obj.check_setup(); tinympc_matlab('set_linear_constraints', Alin_x, blin_x, Alin_u, blin_u, false); @@ -270,7 +254,7 @@ function set_linear_constraints(obj, Alin_x, blin_x, Alin_u, blin_u) end function set_bound_constraints(obj, x_min, x_max, u_min, u_max) - % Set box constraints: x_min <= x <= x_max, u_min <= u <= u_max + % Bounds: x_min <= x <= x_max, u_min <= u <= u_max obj.check_setup(); % Process bounds with simple expansion @@ -294,7 +278,7 @@ function set_bound_constraints(obj, x_min, x_max, u_min, u_max) end function set_cone_constraints(obj, Acu, qcu, cu, Acx, qcx, cx) - % Set second-order cone constraints (inputs first, then states) + % SOC constraints (inputs first, then states) obj.check_setup(); % Convert to proper types: indices to int32, parameters to double if ~isempty(Acu), Acu = int32(Acu(:)); qcu = int32(qcu(:)); cu = double(cu(:)); end @@ -310,8 +294,8 @@ function set_cone_constraints(obj, Acu, qcu, cu, Acx, qcx, cx) end function set_equality_constraints(obj, Aeq_x, beq_x, Aeq_u, beq_u) - % Set equality constraints: Aeq_x * x == beq_x, Aeq_u * u == beq_u - % Implemented internally as paired inequalities in C++ + % Equality constraints: Aeq_x * x == beq_x, Aeq_u * u == beq_u + % Implemented as two inequalities per row and delegated to linear API obj.check_setup(); % Normalize beq inputs to column vectors for safety diff --git a/src/bindings.cpp b/src/bindings.cpp index 0bb9aa4..2c82b95 100644 --- a/src/bindings.cpp +++ b/src/bindings.cpp @@ -186,13 +186,7 @@ void set_u_ref(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { // Set bound constraints void set_bound_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { - if (nrhs != 5) { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "set_bound_constraints requires 5 input arguments"); - } - - if (!g_solver) { - mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); - } + if (!g_solver) mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); auto x_min = matlab_to_eigen(prhs[0]); auto x_max = matlab_to_eigen(prhs[1]); @@ -205,20 +199,13 @@ void set_bound_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* p tinyMatrix u_min_tiny = u_min.cast(); tinyMatrix u_max_tiny = u_max.cast(); - // Use the API function int status = tiny_set_bound_constraints(g_solver.get(), x_min_tiny, x_max_tiny, u_min_tiny, u_max_tiny); - - if (status != 0) { - mexErrMsgIdAndTxt("TinyMPC:SetBoundConstraintsFailed", "tiny_set_bound_constraints failed with status %d", status); - } - + if (status != 0) mexErrMsgIdAndTxt("TinyMPC:SetBoundConstraintsFailed", "status %d", status); + // Auto-enable bound constraints after successful setting g_solver->settings->en_state_bound = 1; g_solver->settings->en_input_bound = 1; - - if (verbose) { - mexPrintf("Bound constraints set\n"); - } + if (verbose) mexPrintf("Bound constraints set\n"); } // Solve the MPC problem (matches Python solve) @@ -419,42 +406,18 @@ void set_cache_terms(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) // Set linear constraints void set_linear_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { - if (nrhs != 5 || !g_solver) { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "Invalid arguments or solver not initialized"); - } - + if (!g_solver) mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); + auto Alin_x = matlab_to_eigen(prhs[0]).cast(); Eigen::MatrixXd blin_x_in = matlab_to_eigen(prhs[1]); auto Alin_u = matlab_to_eigen(prhs[2]).cast(); Eigen::MatrixXd blin_u_in = matlab_to_eigen(prhs[3]); - - // Normalize b vectors to column vectors (Eigen's tinyVector expectation) - Eigen::Matrix blin_x; - if (blin_x_in.size() == 0) { - blin_x.resize(0); - } else if (blin_x_in.cols() == 1) { - blin_x = blin_x_in.cast(); - } else if (blin_x_in.rows() == 1) { - blin_x = blin_x_in.transpose().cast(); - } else { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "blin_x must be a vector (Nx1 or 1xN)"); - } - - Eigen::Matrix blin_u; - if (blin_u_in.size() == 0) { - blin_u.resize(0); - } else if (blin_u_in.cols() == 1) { - blin_u = blin_u_in.cast(); - } else if (blin_u_in.rows() == 1) { - blin_u = blin_u_in.transpose().cast(); - } else { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "blin_u must be a vector (Mx1 or 1xM)"); - } + // Flatten b vectors to column (accept 1xK or Kx1) + Eigen::Matrix blin_x = Eigen::Map(blin_x_in.data(), (int)blin_x_in.size()).cast(); + Eigen::Matrix blin_u = Eigen::Map(blin_u_in.data(), (int)blin_u_in.size()).cast(); int status = tiny_set_linear_constraints(g_solver.get(), Alin_x, blin_x, Alin_u, blin_u); - if (status != 0) { - mexErrMsgIdAndTxt("TinyMPC:SetLinearConstraintsFailed", "Failed with status %d", status); - } + if (status != 0) mexErrMsgIdAndTxt("TinyMPC:SetLinearConstraintsFailed", "status %d", status); // Auto-enable linear constraints after successful setting bool has_state_linear = (Alin_x.rows() > 0 && blin_x.rows() > 0); @@ -485,9 +448,7 @@ Eigen::VectorXi matlab_to_eigen_vectorxi(const mxArray* mx_array) { // Set conic constraints (inputs first, then states) void set_cone_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { - if (nrhs != 7 || !g_solver) { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "Invalid arguments or solver not initialized"); - } + if (!g_solver) mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); auto Acu = matlab_to_eigen_vectorxi(prhs[0]); auto qcu = matlab_to_eigen_vectorxi(prhs[1]); @@ -495,35 +456,13 @@ void set_cone_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* pr auto Acx = matlab_to_eigen_vectorxi(prhs[3]); auto qcx = matlab_to_eigen_vectorxi(prhs[4]); Eigen::MatrixXd cx_in = matlab_to_eigen(prhs[5]); - - // Normalize cu/cx to column vectors (0-length is allowed) - Eigen::VectorXd cu_vec; - if (cu_in.size() == 0) { - cu_vec.resize(0); - } else if (cu_in.cols() == 1) { - cu_vec = cu_in.col(0); - } else if (cu_in.rows() == 1) { - cu_vec = cu_in.transpose().col(0); - } else { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "cu must be a vector (Kx1 or 1xK)"); - } - - Eigen::VectorXd cx_vec; - if (cx_in.size() == 0) { - cx_vec.resize(0); - } else if (cx_in.cols() == 1) { - cx_vec = cx_in.col(0); - } else if (cx_in.rows() == 1) { - cx_vec = cx_in.transpose().col(0); - } else { - mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "cx must be a vector (Kx1 or 1xK)"); - } + // Flatten cu/cx to column vectors (accept 1xK or Kx1) + Eigen::VectorXd cu_vec = Eigen::Map(cu_in.data(), (int)cu_in.size()); + Eigen::VectorXd cx_vec = Eigen::Map(cx_in.data(), (int)cx_in.size()); int status = tiny_set_cone_constraints(g_solver.get(), Acu, qcu, cu_vec.cast(), Acx, qcx, cx_vec.cast()); - if (status != 0) { - mexErrMsgIdAndTxt("TinyMPC:SetConeConstraintsFailed", "Failed with status %d", status); - } + if (status != 0) mexErrMsgIdAndTxt("TinyMPC:SetConeConstraintsFailed", "status %d", status); // Auto-enable cone constraints after successful setting bool has_state_cones = (Acx.size() > 0 && qcx.size() > 0 && cx_vec.size() > 0); From 244fe6c392084f4ae24db1817813412390110070 Mon Sep 17 00:00:00 2001 From: Moises Mata Date: Wed, 20 Aug 2025 18:08:47 -0700 Subject: [PATCH 5/5] swap ordering of cone constraint parameters --- README.md | 4 ++-- examples/rocket_landing_constraints.m | 2 +- src/TinyMPC.m | 8 ++++---- src/bindings.cpp | 16 +++++++++------- 4 files changed, 16 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index 66367f2..a1a643c 100644 --- a/README.md +++ b/README.md @@ -160,8 +160,8 @@ solver.set_bound_constraints(x_min, x_max, u_min, u_max); % Linear inequalities solver.set_linear_constraints(Alin_x, blin_x, Alin_u, blin_u); -% Second-order cones (inputs first, then states) -solver.set_cone_constraints(Acu, qcu, cu, Acx, qcx, cx); +% Second-order cones (states first, then inputs) +solver.set_cone_constraints(Acx, qcx, cx, Acu, qcu, cu); % Equality constraints (implemented as paired inequalities) % Aeq_x * x == beq_x, Aeq_u * u == beq_u diff --git a/examples/rocket_landing_constraints.m b/examples/rocket_landing_constraints.m index 993a3e5..3dfbda7 100644 --- a/examples/rocket_landing_constraints.m +++ b/examples/rocket_landing_constraints.m @@ -54,7 +54,7 @@ solver.set_bound_constraints(x_min, x_max, u_min, u_max); % Set cone constraints -solver.set_cone_constraints(Acu, qcu, cu, Acx, qcx, cx); +solver.set_cone_constraints(Acx, qcx, cx, Acu, qcu, cu); % Initial and goal states xinit = [4.0; 2.0; 20.0; -3.0; 2.0; -4.5]; diff --git a/src/TinyMPC.m b/src/TinyMPC.m index d566216..4e29f47 100644 --- a/src/TinyMPC.m +++ b/src/TinyMPC.m @@ -277,13 +277,13 @@ function set_bound_constraints(obj, x_min, x_max, u_min, u_max) obj.update_settings(); % Push to C++ layer end - function set_cone_constraints(obj, Acu, qcu, cu, Acx, qcx, cx) - % SOC constraints (inputs first, then states) + function set_cone_constraints(obj, Acx, qcx, cx, Acu, qcu, cu) + % SOC constraints (states first, then inputs) obj.check_setup(); % Convert to proper types: indices to int32, parameters to double - if ~isempty(Acu), Acu = int32(Acu(:)); qcu = int32(qcu(:)); cu = double(cu(:)); end if ~isempty(Acx), Acx = int32(Acx(:)); qcx = int32(qcx(:)); cx = double(cx(:)); end - tinympc_matlab('set_cone_constraints', Acu, qcu, cu, Acx, qcx, cx, false); + if ~isempty(Acu), Acu = int32(Acu(:)); qcu = int32(qcu(:)); cu = double(cu(:)); end + tinympc_matlab('set_cone_constraints', Acx, qcx, cx, Acu, qcu, cu, false); % Auto-enable cone constraints after successful setting obj.settings.en_state_soc = ~isempty(Acx) && ~isempty(qcx) && ~isempty(cx); diff --git a/src/bindings.cpp b/src/bindings.cpp index 2c82b95..079f75c 100644 --- a/src/bindings.cpp +++ b/src/bindings.cpp @@ -450,16 +450,18 @@ Eigen::VectorXi matlab_to_eigen_vectorxi(const mxArray* mx_array) { void set_cone_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if (!g_solver) mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); - auto Acu = matlab_to_eigen_vectorxi(prhs[0]); - auto qcu = matlab_to_eigen_vectorxi(prhs[1]); - Eigen::MatrixXd cu_in = matlab_to_eigen(prhs[2]); - auto Acx = matlab_to_eigen_vectorxi(prhs[3]); - auto qcx = matlab_to_eigen_vectorxi(prhs[4]); - Eigen::MatrixXd cx_in = matlab_to_eigen(prhs[5]); + // MATLAB public API: state-first, then input + auto Acx = matlab_to_eigen_vectorxi(prhs[0]); + auto qcx = matlab_to_eigen_vectorxi(prhs[1]); + Eigen::MatrixXd cx_in = matlab_to_eigen(prhs[2]); + auto Acu = matlab_to_eigen_vectorxi(prhs[3]); + auto qcu = matlab_to_eigen_vectorxi(prhs[4]); + Eigen::MatrixXd cu_in = matlab_to_eigen(prhs[5]); // Flatten cu/cx to column vectors (accept 1xK or Kx1) - Eigen::VectorXd cu_vec = Eigen::Map(cu_in.data(), (int)cu_in.size()); Eigen::VectorXd cx_vec = Eigen::Map(cx_in.data(), (int)cx_in.size()); + Eigen::VectorXd cu_vec = Eigen::Map(cu_in.data(), (int)cu_in.size()); + // Core expects input-first, then state int status = tiny_set_cone_constraints(g_solver.get(), Acu, qcu, cu_vec.cast(), Acx, qcx, cx_vec.cast()); if (status != 0) mexErrMsgIdAndTxt("TinyMPC:SetConeConstraintsFailed", "status %d", status);