├── .gitignore ├── Trajectory_Free_Linear_Model_Predictive_Control_for_Stable_Walking_in_the_Presence_of_Strong_Perturbations ├── implementation_technique.md ├── CMakeLists.txt └── generate_trajectory.cpp └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /Trajectory_Free_Linear_Model_Predictive_Control_for_Stable_Walking_in_the_Presence_of_Strong_Perturbations/implementation_technique.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | ## An example of parametors 4 | | | | 5 | | ---- | ---- | 6 | | Control Horizon | 1.5 [s] | 7 | | Unit time | 10 [ms] | 8 | | 𝑄/𝑅 | 100000.0 | 9 | | Height of CoM | 0.6 [m] | 10 | | Step width | 0.15 [m] | 11 | | Upper bound of difference between ref and current output. | 0.02 [m] | 12 | | Lower bound of difference between ref and current output. | -0.02 [m] | 13 | | Upper bound of input | 100 [m/𝑠^3] | 14 | | Lower bound of input | -100 [m/𝑠^3] | 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # humanoid_gait_generation_by_MPC 2 | Here are some examples of walking pattern generators using Model predictive control. 3 | 4 | List of paper which is implemented in this repository. 5 | - P. -b. Wieber, "Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong Perturbations," 2006 6th IEEE-RAS International Conference on Humanoid Robots, Genova, Italy, 2006, pp. 137-142, doi: 10.1109/ICHR.2006.321375. 6 | 7 | 8 | ## Required library 9 | - gnuplot-cpp https://github.com/martinruenz/gnuplot-cpp.git 10 | - Eigen3.4 https://eigen.tuxfamily.org/index.php?title=3.4 11 | - osqp https://osqp.org/ 12 | - osqp-eigen https://github.com/robotology/osqp-eigen.git 13 | 14 | ## How to run 15 | 1. install Eigen3.4 & osqp & osqp-eigen 16 | 2. cd [your target directory] 17 | 3. mkdir build 18 | 4. cmake .. 19 | 5. make 20 | 6. ./exmple 21 | -------------------------------------------------------------------------------- /Trajectory_Free_Linear_Model_Predictive_Control_for_Stable_Walking_in_the_Presence_of_Strong_Perturbations/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(example) 3 | include(FetchContent) 4 | 5 | function(enable_ninja_output_coloring target) 6 | if(UNIX AND CMAKE_GENERATOR STREQUAL "Ninja") 7 | if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") 8 | target_compile_options(${target} PRIVATE -fcolor-diagnostics) 9 | target_compile_options(${target} PRIVATE -fcolor-diagnostics) 10 | endif() 11 | if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") 12 | target_compile_options(${target} PRIVATE -fdiagnostics-color) 13 | target_compile_options(${target} PRIVATE -fdiagnostics-color) 14 | endif() 15 | endif() 16 | endfunction(enable_ninja_output_coloring) 17 | 18 | function(compile_target target) 19 | enable_ninja_output_coloring(${target}) 20 | target_include_directories(${target} PUBLIC Eigen3::Eigen OsqpEigen::OsqpEigen ${osqp_INCLUDE_DIR}) 21 | target_link_libraries(${target} Eigen3::Eigen OsqpEigen::OsqpEigen) 22 | endfunction(compile_target) 23 | 24 | FetchContent_Declare( 25 | gnuplot 26 | GIT_REPOSITORY https://github.com/martinruenz/gnuplot-cpp.git 27 | GIT_TAG origin/master) 28 | FetchContent_MakeAvailable(gnuplot) 29 | 30 | include_directories(${gnuplot_SOURCE_DIR}/include) 31 | message("${gnuplot_SOURCE_DIR}") 32 | message("${gnuplot_BINARY_DIR}") 33 | 34 | set(EIGEN_NO_DEBUG ON) 35 | find_package(Eigen3 REQUIRED) 36 | find_package(OsqpEigen REQUIRED) 37 | 38 | add_executable(example generate_trajectory.cpp) 39 | compile_target(example) 40 | -------------------------------------------------------------------------------- /Trajectory_Free_Linear_Model_Predictive_Control_for_Stable_Walking_in_the_Presence_of_Strong_Perturbations/generate_trajectory.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file generate_trajectory.cpp 3 | * @author Giulio Romualdi , Satoshi Inoue 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2023 6 | * 7 | * 8 | * This code is implementation of this paper. 9 | * P. -b. Wieber, "Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong Perturbations," 10 | * 2006 6th IEEE-RAS International Conference on Humanoid Robots, Genova, Italy, 2006, pp. 137-142, doi: 10.1109/ICHR.2006.321375. 11 | */ 12 | 13 | // osqp-eigen 14 | #include "OsqpEigen/OsqpEigen.h" 15 | 16 | // eigen 17 | #include 18 | 19 | #include 20 | #include 21 | #include "gnuplot.h" 22 | #include 23 | 24 | static constexpr bool enable_sparse_display = false; 25 | // problem setting 26 | static constexpr double step_width = 0.3; // 一歩の大きさ(m) 27 | static constexpr int_fast64_t start_with_this_step = 40; // 10 * T = 0.1秒後に歩き出す 28 | static constexpr int_fast64_t cycle_step = 40; // 何サイクル毎に足踏みするか、 cycle_step * T = 周期(s) 29 | static constexpr int_fast64_t double_support_step = 10; // 両足支持の時間 double_support_step * T = 周期(s) 30 | using namespace Eigen; 31 | 32 | void sparseDisplay(Eigen::SparseMatrix matrix) 33 | { 34 | if (enable_sparse_display) 35 | { 36 | GnuplotPipe gp; 37 | std::ofstream ofs; 38 | ofs.open("sparse_data.dat", std::ios::out); 39 | size_t row = matrix.rows(); 40 | size_t col = matrix.cols(); 41 | for (int k = 0; k < matrix.outerSize(); ++k) 42 | for (SparseMatrix::InnerIterator it(matrix, k); it; ++it) 43 | { 44 | ofs << it.col() << " " << -it.row() << "\n"; 45 | } 46 | gp.sendLine("set terminal wxt size 1280,960"); 47 | // gp.sendLine("set terminal wxt size 640,480"); 48 | gp.sendLine("set xrange [-1:" + std::to_string(col) + "]"); 49 | gp.sendLine("set yrange [-" + std::to_string(row + 1) + ":1]"); 50 | gp.sendLine("plot 'sparse_data.dat' using 1:2"); 51 | } 52 | } 53 | 54 | template 55 | void sparseBlockAssignation(Eigen::SparseMatrix &sparse_mat, const size_t &row_location, const size_t &col_location, const Eigen::MatrixXd &assign_mat) 56 | { 57 | const size_t assign_row = assign_mat.rows(); 58 | const size_t assign_col = assign_mat.cols(); 59 | const size_t sparse_max_col = sparse_mat.cols(); 60 | const size_t sparse_max_row = sparse_mat.rows(); 61 | if ((sparse_max_col < assign_col + col_location) || (sparse_max_row < assign_row + row_location)) 62 | { 63 | std::string msg = "write block will exceed sparse size!!!\n row " + std::to_string(assign_row + row_location) + " col " + std::to_string(assign_col + col_location); 64 | throw std::range_error(msg); 65 | } 66 | for (size_t row = row_location; row < row_location + assign_row; row++) 67 | { 68 | for (size_t col = col_location; col < col_location + assign_col; col++) 69 | { 70 | assert((sparse_max_col >= assign_col + col_location) || (sparse_max_row >= assign_row + row_location)); // over sparse size 71 | sparse_mat.coeffRef(row, col) = assign_mat(row - row_location, col - col_location); 72 | } 73 | } 74 | } 75 | 76 | template 77 | void showTypeName(T &&tp) 78 | { 79 | int tmp = 0; 80 | std::cout << abi::__cxa_demangle(typeid(decltype(std::forward(tp))).name(), 0, 0, &tmp) << std::endl; 81 | } 82 | 83 | void showResult() 84 | { 85 | GnuplotPipe gp; 86 | gp.sendLine("set terminal wxt size 1280,980"); 87 | gp.sendLine("set xrange [0:3]"); 88 | gp.sendLine("set yrange [-0.5:0.5]"); 89 | gp.sendLine("set ylabel 'x(m)' "); 90 | gp.sendLine("set xlabel 't(s)' "); 91 | 92 | gp.sendLine("plot 'x_data.dat' using 1:2 w lp title \" ZMP trajectry \""); 93 | gp.sendLine("replot 'x_data.dat' using 1:3 w lp title \" ref trajectry \""); 94 | gp.sendLine("replot 'x_data.dat' using 1:4 w lp title \" ref max \" "); 95 | gp.sendLine("replot 'x_data.dat' using 1:5 w lp title \" ref min \" "); 96 | gp.sendLine("replot 'x_data.dat' using 1:6 w lp title \" x \" "); 97 | } 98 | 99 | Eigen::VectorXd generateRefTrajectory(const int32_t &step, const int32_t &horizon_length, const double &step_width, const int32_t &step_cycle) 100 | { 101 | static constexpr double T = 0.01; // サンプリング周期 (s) 102 | // static constexpr int_fast64_t start_step = 20; // 10 * T = 0.1秒後に歩き出す 103 | // static constexpr int_fast64_t cycle_step = 40; // 何サイクル毎に足踏みするか、 cycle_step * T = 周期(s) 104 | // static constexpr double step_width = 0.3; // 一歩の大きさ(m) 105 | Eigen::VectorXd ret = Eigen::VectorXd::Zero(horizon_length); 106 | for (int32_t i = 0; i < horizon_length; i++) 107 | { 108 | if ((step + i) >= start_with_this_step) 109 | { 110 | if (((step + i) / step_cycle) % 2 == 0) 111 | { 112 | ret(i, 0) = step_width; 113 | } 114 | else 115 | { 116 | ret(i, 0) = -step_width; 117 | } 118 | } 119 | } 120 | return ret; 121 | } 122 | 123 | template 124 | void castMPCToQPHessian(const Eigen::DiagonalMatrix &Q, const Eigen::DiagonalMatrix &R, 125 | Eigen::Matrix &C, Eigen::SparseMatrix &hessianMatrix) 126 | { 127 | 128 | hessianMatrix.resize(X_SIZE * (mpcWindow + 1) + U_SIZE * mpcWindow, X_SIZE * (mpcWindow + 1) + U_SIZE * mpcWindow); 129 | 130 | // populate hessian matrix 131 | //QPの変数を最適入力だけでなく、状態も含め、 x = [x(k) ... x(k+N) u(k+1) ... u(k+N)]^T としているため、hessianはQとRが分離した形の対角行列として表せる。 132 | //ここでは、状態ではなく出力を追従させたい為、Cを掛けておく。 133 | for (int i = 0; i < X_SIZE * (mpcWindow + 1) + U_SIZE * mpcWindow; i++) 134 | { 135 | if (i < X_SIZE * (mpcWindow + 1)) 136 | { 137 | if (i % X_SIZE == 0) 138 | { 139 | int posQ = i / X_SIZE; 140 | float value = Q.diagonal()[posQ] / 2.0f; 141 | Eigen::MatrixXd C_tC = C.transpose() * C * value; 142 | sparseBlockAssignation(hessianMatrix, i, i, C_tC); 143 | } 144 | } 145 | else 146 | { 147 | int posR = i % U_SIZE; 148 | float value = R.diagonal()[posR] /2.0f; 149 | if (value != 0) 150 | hessianMatrix.insert(i, i) = value; 151 | } 152 | } 153 | 154 | } 155 | 156 | 157 | template 158 | void castMPCToQPGradient(const Eigen::DiagonalMatrix &Q, const Eigen::Matrix &zRef, 159 | const Eigen::Matrix &C, Eigen::VectorXd &gradient) 160 | { 161 | 162 | Eigen::Matrix Qz_ref; 163 | Qz_ref = Q * (-zRef); 164 | // populate the gradient vector 165 | gradient = Eigen::VectorXd::Zero(X_SIZE * (mpcWindow + 1) + U_SIZE * mpcWindow, 1); // ここでのX_SIZEはC_SIZEを表す 166 | //QPの変数を最適入力だけでなく、状態も含め、 x = [x(k) ... x(k+N) u(k+1) ... u(k+N)]^T としているため、以下の単純な形でgradientを表す事が出来る。 167 | for (int i = 0; i * X_SIZE < X_SIZE * (mpcWindow + 1); i++) 168 | { 169 | int posQ = i; 170 | float value = Qz_ref(posQ, 0); 171 | //今回、目標に追従するのは状態ではなく出力であるのでCを掛ける必要あり 172 | gradient.block(i * X_SIZE, 0, X_SIZE, 1) = value * C.transpose(); 173 | } 174 | } 175 | 176 | template 177 | void castMPCToQPConstraintMatrix(const Eigen::Matrix &dynamicMatrix, const Eigen::Matrix &controlMatrix, 178 | const Eigen::Matrix &outputMatrix, Eigen::SparseMatrix &constraintMatrix) 179 | { 180 | constraintMatrix.resize(X_SIZE * (mpcWindow + 1) + Z_SIZE * (mpcWindow + 1) + U_SIZE * mpcWindow, X_SIZE * (mpcWindow + 1) + U_SIZE * mpcWindow); 181 | // 　↑の1つめの　X_SIZE * (mpcWindow + 1) = QPのxの中に状態を持たせる為に使う変数の分。 x0を不等式制約とし、 182 | // そのx0を使ってそれ以降のダイナミクスを順々に計算し、それを = 0の等式制約とすると、 183 | // なんとx(n) = Ax(n-1) + Bu(n-1)の予測の式を制約上で表す事ができ、QPのxの中に状態を変数として導入出来る 184 | // 　↑の2つめの　Z_SIZE * (mpcWindow + 1) = 出力の不等式制約 185 | // 　↑の3つめの　U_SIZE * mpcWindow = 入力の不等式制約 186 | 187 | // populate linear constraint matrix 188 | // 状態の所の-Iを代入 189 | for (int i = 0; i < X_SIZE * (mpcWindow + 1); i++) 190 | { 191 | constraintMatrix.insert(i, i) = -1; 192 | } 193 | // 状態の所のAを代入 194 | for (int i = 0; i < mpcWindow; i++) 195 | for (int j = 0; j < X_SIZE; j++) 196 | for (int k = 0; k < X_SIZE; k++) 197 | { 198 | float value = dynamicMatrix(j, k); 199 | if (value != 0) 200 | { 201 | constraintMatrix.insert(X_SIZE * (i + 1) + j, X_SIZE * i + k) = value; 202 | } 203 | } 204 | // 状態の所のBを代入 205 | for (int i = 0; i < mpcWindow; i++) 206 | for (int j = 0; j < X_SIZE; j++) 207 | for (int k = 0; k < U_SIZE; k++) 208 | { 209 | float value = controlMatrix(j, k); 210 | if (value != 0) 211 | { 212 | constraintMatrix.insert(X_SIZE * (i + 1) + j, U_SIZE * i + k + X_SIZE * (mpcWindow + 1)) = value; 213 | } 214 | } 215 | 216 | // zの不等式制約のCを代入 217 | for (int i = 0; i < Z_SIZE * (mpcWindow + 1); ++i) 218 | { 219 | sparseBlockAssignation(constraintMatrix, (i * Z_SIZE + (mpcWindow + 1) * X_SIZE), i * X_SIZE, outputMatrix); 220 | } 221 | 222 | // uの不等式制約のIを代入 223 | for (int i = X_SIZE * (mpcWindow + 1); i < X_SIZE * (mpcWindow + 1) + U_SIZE * mpcWindow; i++) 224 | { 225 | constraintMatrix.insert(i + Z_SIZE * (mpcWindow + 1), i) = 1; 226 | } 227 | sparseDisplay(constraintMatrix); 228 | } 229 | 230 | template 231 | void castMPCToQPConstraintVectors(const Eigen::Matrix &zMax, const Eigen::Matrix &zMin, 232 | const Eigen::Matrix &double_spt_zMax, const Eigen::Matrix &double_spt_zMin, 233 | const Eigen::Matrix &uMax, const Eigen::Matrix &uMin, 234 | const Eigen::Matrix &x0, const Eigen::Matrix &zRef, 235 | Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound) 236 | { 237 | // evaluate the lower and the upper inequality vectors 238 | Eigen::VectorXd lowerInequality = Eigen::MatrixXd::Zero(Z_SIZE * (mpcWindow + 1) + U_SIZE * mpcWindow, 1); 239 | Eigen::VectorXd upperInequality = Eigen::MatrixXd::Zero(Z_SIZE * (mpcWindow + 1) + U_SIZE * mpcWindow, 1); 240 | for (int i = 0; i < mpcWindow + 1; i++) 241 | { 242 | 243 | if (((i) % cycle_step < (double_support_step / 2)) || ((i) % cycle_step >= cycle_step - (double_support_step / 2)) || (i <= start_with_this_step + double_support_step / 2)) 244 | { 245 | lowerInequality.block(i * Z_SIZE, 0, Z_SIZE, 1) = double_spt_zMin; 246 | upperInequality.block(i * Z_SIZE, 0, Z_SIZE, 1) = double_spt_zMax; 247 | } 248 | else // 片足支持の時 249 | { 250 | // 対象のzRefが正の場合 なお、符号など気にせずそのまま足せば良かった模様 251 | lowerInequality.block(i * Z_SIZE, 0, Z_SIZE, 1) = zMin + zRef.block(i, 0, Z_SIZE, 1); 252 | upperInequality.block(i * Z_SIZE, 0, Z_SIZE, 1) = zMax + zRef.block(i, 0, Z_SIZE, 1); 253 | } 254 | } 255 | 256 | for (int i = 0; i < mpcWindow; i++) 257 | { 258 | lowerInequality.block(U_SIZE * i + Z_SIZE * (mpcWindow + 1), 0, U_SIZE, 1) = uMin; 259 | upperInequality.block(U_SIZE * i + Z_SIZE * (mpcWindow + 1), 0, U_SIZE, 1) = uMax; 260 | } 261 | 262 | // evaluate the lower and the upper equality vectors 263 | Eigen::VectorXd lowerEquality = Eigen::MatrixXd::Zero(X_SIZE * (mpcWindow + 1), 1); 264 | Eigen::VectorXd upperEquality; 265 | lowerEquality.block(0, 0, X_SIZE, 1) = -x0; 266 | upperEquality = lowerEquality; 267 | lowerEquality = lowerEquality; 268 | 269 | // merge inequality and equality vectors 270 | lowerBound = Eigen::MatrixXd::Zero(X_SIZE * (mpcWindow + 1) + Z_SIZE * (mpcWindow + 1) + U_SIZE * mpcWindow, 1); 271 | lowerBound << lowerEquality, 272 | lowerInequality; 273 | 274 | upperBound = Eigen::MatrixXd::Zero(X_SIZE * (mpcWindow + 1) + Z_SIZE * (mpcWindow + 1) + U_SIZE * mpcWindow, 1); 275 | upperBound << upperEquality, 276 | upperInequality; 277 | } 278 | 279 | template 280 | void updateConstraintVectors(const Eigen::Matrix &x0, const Eigen::Matrix &zRef, 281 | const Eigen::Matrix &zMax, const Eigen::Matrix &zMin, 282 | const Eigen::Matrix &double_spt_zMax, const Eigen::Matrix &double_spt_zMin, 283 | Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound, const int32_t current_step) 284 | { 285 | lowerBound.block(0, 0, X_SIZE, 1) = -x0; 286 | upperBound.block(0, 0, X_SIZE, 1) = -x0; 287 | for (size_t i = 0; i < mpcWindow + 1; i++) 288 | { 289 | // 両足支持期間 290 | if (((current_step + i) % cycle_step < (double_support_step / 2)) || ((current_step + i) % cycle_step >= cycle_step - (double_support_step / 2)) || (i + current_step <= start_with_this_step + double_support_step / 2)) 291 | { 292 | lowerBound.block(i + X_SIZE * (1 + mpcWindow), 0, Z_SIZE, 1) = double_spt_zMin; 293 | upperBound.block(i + X_SIZE * (1 + mpcWindow), 0, Z_SIZE, 1) = double_spt_zMax; 294 | } 295 | else // 片足支持の時 296 | { 297 | // 対象のzRefが正の場合 なお、符号など気にせずそのまま足せば良かった模様 298 | lowerBound.block(i + X_SIZE * (1 + mpcWindow), 0, Z_SIZE, 1) = zMin + zRef.block(i * Z_SIZE, 0, Z_SIZE, 1); 299 | upperBound.block(i + X_SIZE * (1 + mpcWindow), 0, Z_SIZE, 1) = zMax + zRef.block(i * Z_SIZE, 0, Z_SIZE, 1); 300 | } 301 | } 302 | } 303 | 304 | template 305 | double getErrorNorm(const Eigen::Matrix &z, 306 | const Eigen::Matrix &zRef) 307 | { 308 | // evaluate the error 309 | Eigen::Matrix error = z - zRef; 310 | 311 | // return the norm 312 | return error.norm(); 313 | } 314 | 315 | int main() 316 | { 317 | // set the preview window 318 | static constexpr double hCoM = 0.6; 319 | static constexpr double g = 9.81; 320 | static constexpr double T = 0.01; 321 | 322 | static constexpr int32_t mpcWindow = 150; // horizon length 323 | // number of iteration steps 324 | static constexpr int32_t numberOfSteps = 600; 325 | static constexpr int32_t Mu = 1; 326 | static constexpr int32_t Nx = 3; 327 | static constexpr int32_t Zx = 1; 328 | static constexpr int32_t num_of_variables = Nx * (numberOfSteps + 1) + Mu * numberOfSteps; 329 | 330 | static constexpr double Q_scale = 100000; 331 | static constexpr double R_scale = 1; 332 | 333 | // allocate the dynamics matrices 334 | Eigen::Matrix A; 335 | A << 1.0f, T, T * T / 2.0f, 336 | 0, 1.0f, T, 337 | 0, 0, 1.0f; 338 | Eigen::Matrix B; 339 | B << T * T * T / 6.0f, 340 | T * T / 2.0f, 341 | T; 342 | Eigen::Matrix C; 343 | C << 1.0f, 0, -hCoM / g; 344 | 345 | // allocate the constraints vector 346 | Eigen::Matrix zMax; // 今の所　正 347 | Eigen::Matrix zMin; // 今の所　負 348 | Eigen::Matrix double_spport_zMax; 349 | Eigen::Matrix double_spport_zMin; 350 | Eigen::Matrix uMax; 351 | Eigen::Matrix uMin; 352 | zMax << 0.02; 353 | zMin << -0.02; 354 | double_spport_zMax << zMax(0, 0) + step_width; 355 | double_spport_zMin << zMin(0, 0) - step_width; 356 | uMax << 100; 357 | uMin << -100; 358 | 359 | // allocate the weight matrices 360 | // ホライゾン長に渡る、書く予測ステップ毎のZxに対するコスト。ここではZxが1次元なので、mpcWindow + 1の数がQのサイズになる。 + 1してるのは状態にx0が入っている為。 361 | Eigen::DiagonalMatrix Q; 362 | // uに掛けるコスト。こちらはホライゾン長に渡って共通のものを掛ける。サイズ = Mu 363 | Eigen::DiagonalMatrix R; 364 | Q.setIdentity(); 365 | R.setIdentity(); 366 | Q = Q * Q_scale; 367 | R = R * R_scale; 368 | 369 | // allocate the initial and the reference state space 370 | Eigen::Matrix x0; 371 | Eigen::Matrix zRef; 372 | 373 | // allocate QP problem matrices and vectores 374 | Eigen::SparseMatrix hessian; 375 | Eigen::VectorXd gradient; 376 | Eigen::SparseMatrix linearMatrix; 377 | Eigen::VectorXd lowerBound; 378 | Eigen::VectorXd upperBound; 379 | 380 | // set the initial and the desired states 381 | x0 << 0, 0, 0; 382 | zRef = generateRefTrajectory(0, mpcWindow + 1, step_width, cycle_step); 383 | 384 | // cast the MPC problem as QP problem 385 | castMPCToQPHessian(Q, R, C, hessian); 386 | castMPCToQPGradient(Q, zRef, C, gradient); 387 | castMPCToQPConstraintMatrix(A, B, C, linearMatrix); 388 | castMPCToQPConstraintVectors(zMax, zMin, double_spport_zMax, double_spport_zMin, uMax, uMin, x0, zRef, lowerBound, upperBound); 389 | 390 | // // instantiate the solver 391 | OsqpEigen::Solver solver; 392 | 393 | // settings 394 | solver.settings()->setWarmStart(false); 395 | 396 | // set the initial data of the QP solver 397 | 398 | //ここではQPの変数を求める最適入力だけとするのではなく、x(0) ~ x(k+N)までのN+1個の状態も含める。 399 | //変数のベクトルは x = [x(k) ... x(k+N) u(k+1) ... u(k+N)]^T という形のベクトルとなる。 400 | solver.data()->setNumberOfVariables(Nx * (mpcWindow + 1) + Mu * mpcWindow); 401 | 402 | solver.data()->setNumberOfConstraints(Nx * (mpcWindow + 1) + Zx * (mpcWindow + 1) + Mu * mpcWindow); 403 | if (!solver.data()->setHessianMatrix(hessian)) 404 | return 1; 405 | if (!solver.data()->setGradient(gradient)) 406 | return 1; 407 | if (!solver.data()->setLinearConstraintsMatrix(linearMatrix)) 408 | return 1; 409 | if (!solver.data()->setLowerBound(lowerBound)) 410 | return 1; 411 | if (!solver.data()->setUpperBound(upperBound)) 412 | return 1; 413 | 414 | // instantiate the solver 415 | if (!solver.initSolver()) 416 | return 1; 417 | // controller input and QPSolution vector 418 | Eigen::Matrix ctr; 419 | Eigen::VectorXd QPSolution; 420 | 421 | std::ofstream ofs; 422 | ofs.open("x_data.dat"); 423 | auto updateGradient = [&](const size_t &i) 424 | { 425 | zRef = generateRefTrajectory(i, mpcWindow + 1, step_width, cycle_step); 426 | castMPCToQPGradient(Q, zRef, C, gradient); 427 | }; 428 | 429 | for (int i = 0; i < numberOfSteps; i++) 430 | { 431 | 432 | // solve the QP problem 433 | if (solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError) 434 | return 1; 435 | 436 | if (solver.getStatus() != OsqpEigen::Status::Solved) 437 | { 438 | std::cout << ctr << std::endl; 439 | std::cout << "======== some problems occured !!!========" << std::endl; 440 | std::cout << "solver status is ::: " << static_cast(solver.getStatus()) << std::endl; 441 | std::cout << "this happens on step " << i << std::endl; 442 | return 1; 443 | } 444 | // get the controller input 445 | QPSolution = solver.getSolution(); 446 | ctr = QPSolution.block(Nx * (mpcWindow + 1), 0, Mu, 1); 447 | 448 | // propagate the model 449 | x0 = A * x0 + B * ctr; 450 | 451 | // save data into file 452 | std::cout << x0 << std::endl; 453 | ofs << i * T << " " << C * x0 << " " << zRef(0, 0) << " " << upperBound(Nx * (mpcWindow + 1), 0) << " " << lowerBound(Nx * (mpcWindow + 1), 0) << " " << x0(0, 0) << std::endl; 454 | // update gradient 455 | zRef = generateRefTrajectory(i, mpcWindow + 1, step_width, cycle_step); 456 | castMPCToQPGradient(Q, zRef, C, gradient); 457 | 458 | // update the constraint bound 459 | updateConstraintVectors(x0, zRef, zMax, zMin, double_spport_zMax, double_spport_zMin, lowerBound, upperBound, i); 460 | 461 | if (!solver.updateBounds(lowerBound, upperBound)) 462 | return 1; 463 | if (i == numberOfSteps - 1 ) 464 | { 465 | std::cout << "----answer----" << std::endl; 466 | std::cout << QPSolution << std::endl; 467 | std::cout << "answer cols " << QPSolution.cols() << " rows " << QPSolution.rows() << std::endl; 468 | std::cout << " control \n" 469 | << ctr << std::endl; 470 | } 471 | } 472 | showResult(); 473 | return 0; 474 | } 475 | --------------------------------------------------------------------------------