diff --git a/README.md b/README.md index 6025c23..a1a643c 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 (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 +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 new file mode 100644 index 0000000..3dfbda7 --- /dev/null +++ b/examples/rocket_landing_constraints.m @@ -0,0 +1,174 @@ +%% Rocket Landing with Constraints +% Based on: https://github.com/TinyMPC/TinyMPC/blob/main/examples/rocket_landing_mpc.cpp + +% Add TinyMPC class to path +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 +Acu = [0]; % start indices for input cones +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, ... + '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(Acx, qcx, cx, Acu, qcu, cu); + +% 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 +x_ref = zeros(NSTATES, NHORIZON); +u_ref = zeros(NINPUTS, NHORIZON-1); + +NTOTAL = 100; +x_current = xinit_scaled; + +% Set initial reference +for i = 1:NHORIZON + x_ref(:, i) = xinit + (xgoal - xinit) * (i-1) / (NTOTAL - 1); +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 + 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 + 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..4e29f47 100644 --- a/src/TinyMPC.m +++ b/src/TinyMPC.m @@ -29,6 +29,10 @@ obj.settings.check_termination = 1; 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,7 +56,6 @@ 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', false, 'en_input_bound', false, ... 'adaptive_rho', false, 'adaptive_rho_min', 0.1, 'adaptive_rho_max', 10.0, ... @@ -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 @@ -110,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(); @@ -122,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(); @@ -134,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}) @@ -150,8 +133,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) @@ -256,6 +240,82 @@ 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) + % 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) + % Bounds: 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, 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(Acx), Acx = int32(Acx(:)); qcx = int32(qcx(:)); cx = double(cx(:)); end + 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); + 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) + % 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 + 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) % Reset solver if obj.is_setup diff --git a/src/bindings.cpp b/src/bindings.cpp index d8f13c5..079f75c 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,15 +184,9 @@ 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"); - } - - 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]); @@ -229,16 +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 (same as Python) 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 (verbose) { - mexPrintf("Bound constraints set\n"); - } + 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"); } // Solve the MPC problem (matches Python solve) @@ -437,6 +404,79 @@ 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 (!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]); + // 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", "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); + 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"); + } + return result; +} + +// Set conic constraints (inputs first, then states) +void set_cone_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { + if (!g_solver) mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); + + // 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 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); + + // 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) void codegen_with_sensitivity(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if (nrhs != 6) { @@ -506,8 +546,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) { @@ -521,11 +561,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) { @@ -536,6 +580,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; @@ -629,6 +677,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); }