TinyMPC项目代码

#include <stdio.h> #include <ctype.h> #include <time.h> #include <dirent.h> #include <sys/types.h> #include <sys/stat.h> #include <unistd.h> #include <iostream> #include <Eigen/Dense> // #include "types.hpp" #include "codegen.hpp" #ifdef __cplusplus extern "C" { #endif /* Define the maximum allowed length of the path (directory + filename + extension) */ #define PATH_LENGTH 2048 /* Define the maximum allowed length of the dirname */ #define DIR_NAME_LENGTH 100 /* Define the maximum allowed length of the filename (no extension)*/ #define FILE_LENGTH 100 #define BUF_SIZE 65536 using namespace Eigen; IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]"); static void copy_file(char *src_file_name, char *dest_file_name) { FILE *src_f, *dst_f; src_f = fopen(src_file_name, "r"); if (src_f == NULL) printf("ERROR OPENING SOLVER SOURCE FILE\n"); // return tiny_error(TINY_FOPEN_ERROR); dst_f = fopen(dest_file_name, "w+"); if (dst_f == NULL) printf("ERROR OPENING SOLVER DESTINATION FILE\n"); // return tiny_error(TINY_FOPEN_ERROR); char c; // Copy contents of solver to code generated solver file c = fgetc(src_f); while (c != EOF) { fputc(c, dst_f); c = fgetc(src_f); } fclose(src_f); fclose(dst_f); } static void copy_dir(char *src_dir_name, char *dest_dir_name) { // Open folder at given directory path DIR *src_dir = opendir(src_dir_name); if (src_dir == NULL) { printf("SOURCE DIRECTORY NAME DOES NOT EXIST\n"); return; } struct dirent *dir_entry; // printf("reading source directory %s\n", src_dir_name); char dir_entry_full_name[PATH_LENGTH + DIR_NAME_LENGTH]; char codegen_dest_file_name[PATH_LENGTH + DIR_NAME_LENGTH]; struct stat buffer; while (dir_entry = readdir(src_dir)) { // printf("dir_entry->d_name: %s\n", dir_entry->d_name); sprintf(dir_entry_full_name, "%s/%s", src_dir_name, dir_entry->d_name); stat(dir_entry_full_name, &buffer); // printf("dir_entry_full_name: %s\n", dir_entry_full_name); sprintf(codegen_dest_file_name, "%s/%s", dest_dir_name, dir_entry->d_name); // printf("codegen_dest_file_name: %s\n", codegen_dest_file_name); // Check if file is directory or not if (S_ISDIR(buffer.st_mode) && strcmp(dir_entry->d_name, "..") != 0 && strcmp(dir_entry->d_name, ".") != 0) { // If directory entry is another directory, create that folder // in the codegen directory and call copy_dir again // printf("%s is a directory\n", dir_entry->d_name); struct stat st = {0}; if (stat(codegen_dest_file_name, &st) == -1) { // printf("Creating new folder %s\n", codegen_dest_file_name); mkdir(codegen_dest_file_name, 0700); } copy_dir(dir_entry_full_name, codegen_dest_file_name); } else if (!S_ISDIR(buffer.st_mode)) { // Otherwise, copy file into code gen directory // printf("%s is not a directory\n", dir_entry->d_name); copy_file(dir_entry_full_name, codegen_dest_file_name); } } closedir(src_dir); } static void print_matrix(FILE *f, MatrixXd mat, int num_elements) { for (int i = 0; i < num_elements; i++) { fprintf(f, "(tinytype)%.16f", mat.reshaped<RowMajor>()[i]); if (i < num_elements - 1) fprintf(f, ","); } } static void codegen_example_main(time_t start_time, const char *codegen_dname) { // Write main function char main_fname[PATH_LENGTH + DIR_NAME_LENGTH + FILE_LENGTH]; FILE *main_f; sprintf(main_fname, "%s/tiny_main.cpp", codegen_dname); // Open global options file main_f = fopen(main_fname, "w+"); if (main_f == NULL) printf("ERROR OPENING EXAMPLE MAIN FILE\n"); // return tiny_error(TINY_FOPEN_ERROR); // Preamble time(&start_time); fprintf(main_f, "/*\n"); fprintf(main_f, " * This file was autogenerated by TinyMPC on %s", ctime(&start_time)); fprintf(main_f, " */\n\n"); fprintf(main_f, "#include <iostream>\n\n"); fprintf(main_f, "#include <tinympc/admm.hpp>\n"); fprintf(main_f, "#include <tinympc/tiny_data_workspace.hpp>\n\n"); fprintf(main_f, "using namespace Eigen;\n"); fprintf(main_f, "IOFormat CleanFmt(4, 0, \", \", \"\\n\", \"[\", \"]\");\n\n"); fprintf(main_f, "#ifdef __cplusplus\n"); fprintf(main_f, "extern \"C\" {\n"); fprintf(main_f, "#endif\n\n"); fprintf(main_f, "int main()\n"); fprintf(main_f, "{\n"); fprintf(main_f, "\tint exitflag = 1;\n"); fprintf(main_f, "\t// Double check some data\n"); fprintf(main_f, "\tstd::cout << tiny_data_solver.settings->max_iter << std::endl;\n"); fprintf(main_f, "\tstd::cout << tiny_data_solver.cache->AmBKt.format(CleanFmt) << std::endl;\n"); fprintf(main_f, "\tstd::cout << tiny_data_solver.work->Adyn.format(CleanFmt) << std::endl;\n\n"); fprintf(main_f, "\texitflag = tiny_solve(&tiny_data_solver);\n\n"); fprintf(main_f, "\tif (exitflag == 0) printf(\"HOORAY! Solved with no error!\\n\");\n"); fprintf(main_f, "\telse printf(\"OOPS! Something went wrong!\\n\");\n"); fprintf(main_f, "\treturn 0;\n"); fprintf(main_f, "}\n\n"); fprintf(main_f, "#ifdef __cplusplus\n"); fprintf(main_f, "} /* extern \"C\" */\n"); fprintf(main_f, "#endif\n"); // Close codegen example main file fclose(main_f); printf("Example tinympc main generated in %s\n", main_fname); } int tiny_codegen(const int nx, const int nu, const int N, double *Adyn_data, double *Bdyn_data, double *Q_data, double *R_data, double *x_min_data, double *x_max_data, double *u_min_data, double *u_max_data, double rho, double abs_pri_tol, double abs_dua_tol, int max_iters, int check_termination, int gen_wrapper, const char *tinympc_dir, const char *output_dir) { int en_state_bound = 0; int en_input_bound = 0; if (x_min_data != nullptr && x_max_data != nullptr) { en_state_bound = 1; } else { en_state_bound = 0; } if (u_min_data != nullptr && u_max_data != nullptr) { en_input_bound = 1; } else { en_input_bound = 0; } MatrixXd Adyn = MatrixXd::Map(Adyn_data, nx, nx); MatrixXd Bdyn = MatrixXd::Map(Bdyn_data, nx, nu); MatrixXd Q = MatrixXd::Map(Q_data, nx, 1); MatrixXd R = MatrixXd::Map(R_data, nu, 1); MatrixXd x_min = MatrixXd::Map(x_min_data, nx, N); MatrixXd x_max = MatrixXd::Map(x_max_data, nx, N); MatrixXd u_min = MatrixXd::Map(u_min_data, nu, N - 1); MatrixXd u_max = MatrixXd::Map(u_max_data, nu, N - 1); // Update by adding rho * identity matrix to Q, R Q = Q + rho * MatrixXd::Ones(nx, 1); R = R + rho * MatrixXd::Ones(nu, 1); MatrixXd Q1 = Q.array().matrix().asDiagonal(); MatrixXd R1 = R.array().matrix().asDiagonal(); // Printing std::cout << "A = " << Adyn.format(CleanFmt) << std::endl; std::cout << "B = " << Bdyn.format(CleanFmt) << std::endl; std::cout << "Q = " << Q1.format(CleanFmt) << std::endl; std::cout << "R = " << R1.format(CleanFmt) << std::endl;