TinyMPC的MATLAB交互界面_MATLAB interactive interface for TinyMPC.

#include "mex.h" #include "matrix.h" #include <iostream> #include <cstring> #include <memory> // Fix the Eigen include issue by using the standard approach #include <Eigen/Dense> #include <Eigen/Core> // Now include TinyMPC headers (after Eigen is properly included) #include "tinympc/tiny_api.hpp" #include "tinympc/types.hpp" #include "tinympc/codegen.hpp" // Global solver pointer - matches Python structure static std::unique_ptr<TinySolver> g_solver = nullptr; // Helper function to convert MATLAB array to Eigen matrix Eigen::MatrixXd matlab_to_eigen(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); return Eigen::Map<Eigen::MatrixXd>(data, rows, cols); } // Helper function to convert Eigen matrix to MATLAB array mxArray* eigen_to_matlab(const Eigen::MatrixXd& eigen_mat) { size_t rows = eigen_mat.rows(); size_t cols = eigen_mat.cols(); mxArray* mx_array = mxCreateDoubleMatrix(rows, cols, mxREAL); double* data = mxGetPr(mx_array); // Copy data (Eigen is column-major, MATLAB is column-major, so direct copy) std::memcpy(data, eigen_mat.data(), rows * cols * sizeof(double)); return mx_array; } // 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, verbose if (nrhs != 10) { mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "setup requires 10 input arguments: A, B, fdyn, Q, R, rho, nx, nu, N, verbose"); } // Extract matrices auto A = matlab_to_eigen(prhs[0]); auto B = matlab_to_eigen(prhs[1]); auto fdyn = matlab_to_eigen(prhs[2]); auto Q = matlab_to_eigen(prhs[3]); auto R = matlab_to_eigen(prhs[4]); double rho = mxGetScalar(prhs[5]); // 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[9]); if (verbose) { mexPrintf("Setting up TinyMPC solver with nx=%d, nu=%d, N=%d, rho=%f\n", nx, nu, N, rho); } try { // Create solver using exact same process as Python PyTinySolver constructor TinySolver* solver_ptr = nullptr; // Convert Eigen matrices to tinyMatrix (matching Python bindings) tinyMatrix A_tiny = A.cast<tinytype>(); tinyMatrix B_tiny = B.cast<tinytype>(); tinyMatrix fdyn_tiny = fdyn.cast<tinytype>(); tinyMatrix Q_tiny = Q.cast<tinytype>(); tinyMatrix R_tiny = R.cast<tinytype>(); // Setup solver int status = tiny_setup(&solver_ptr, A_tiny, B_tiny, fdyn_tiny, Q_tiny, R_tiny, (tinytype)rho, nx, nu, N, verbose); if (status != 0) { mexErrMsgIdAndTxt("TinyMPC:SetupFailed", "tiny_setup failed with status %d", status); } // Store solver (transfer ownership) g_solver.reset(solver_ptr); if (verbose) { mexPrintf("TinyMPC solver setup completed successfully\n"); } // Return status (0 for success) plhs[0] = mxCreateDoubleScalar(0); } catch (const std::exception& e) { mexErrMsgIdAndTxt("TinyMPC:SetupException", "Setup failed: %s", e.what()); } } // Set initial state void set_x0(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if (nrhs != 2) { mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "set_x0 requires 2 input arguments"); } if (!g_solver) { mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); } auto x0 = matlab_to_eigen(prhs[0]); int verbose = (int)mxGetScalar(prhs[1]); tinyVector x0_tiny = x0.cast<tinytype>(); // Use the API function int status = tiny_set_x0(g_solver.get(), x0_tiny); if (status != 0) { mexErrMsgIdAndTxt("TinyMPC:SetX0Failed", "tiny_set_x0 failed with status %d", status); } if (verbose) { mexPrintf("Initial state set\n"); } } // Set state reference void set_x_ref(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if (nrhs != 2) { mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "set_x_ref requires 2 input arguments"); } if (!g_solver) { mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); } auto x_ref = matlab_to_eigen(prhs[0]); int verbose = (int)mxGetScalar(prhs[1]); tinyMatrix x_ref_tiny = x_ref.cast<tinytype>(); // Use the API function int status = tiny_set_x_ref(g_solver.get(), x_ref_tiny); if (status != 0) { mexErrMsgIdAndTxt("TinyMPC:SetXRefFailed", "tiny_set_x_ref failed with status %d", status); } if (verbose) { mexPrintf("State reference set\n"); } } // Set input reference void set_u_ref(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if (nrhs != 2) { mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "set_u_ref requires 2 input arguments"); } if (!g_solver) { mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); } auto u_ref = matlab_to_eigen(prhs[0]); int verbose = (int)mxGetScalar(prhs[1]); tinyMatrix u_ref_tiny = u_ref.cast<tinytype>(); // Use the API function int status = tiny_set_u_ref(g_solver.get(), u_ref_tiny); if (status != 0) { mexErrMsgIdAndTxt("TinyMPC:SetURefFailed", "tiny_set_u_ref failed with status %d", status); } if (verbose) { mexPrintf("Input reference set\n"); } } // Set bound constraints void set_bound_constraints(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { 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]); auto u_min = matlab_to_eigen(prhs[2]); auto u_max = matlab_to_eigen(prhs[3]); int verbose = (int)mxGetScalar(prhs[4]); tinyMatrix x_min_tiny = x_min.cast<tinytype>(); tinyMatrix x_max_tiny = x_max.cast<tinytype>(); tinyMatrix u_min_tiny = u_min.cast<tinytype>(); tinyMatrix u_max_tiny = u_max.cast<tinytype>(); 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", "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) void solve_mpc(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if (nrhs != 1) { mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "solve requires 1 input argument"); } if (!g_solver) { mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not initialized"); } int verbose = (int)mxGetScalar(prhs[0]); // Solve the problem (exactly like Python PyTinySolver::solve) int status = tiny_solve(g_solver.get()); if (verbose) { mexPrintf("Solve completed with status: %d\n", status); } // Return status (always 0 in Python bindings) plhs[0] = mxCreateDoubleScalar(0); } // Get solution void get_solution(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { if (nrhs != 1) { mexErrMsgIdAndTxt("TinyMPC:InvalidInput", "get_solution requires 1 input argument"); } if (!g_solver) { mexErrMsgIdAndTxt("TinyMPC:NotInitialized", "Solver not in