├── README.md ├── LICENSE └── nmpc └── cgmres ├── main_example.py └── main_two_wheeled.py /README.md: -------------------------------------------------------------------------------- 1 | [![MIT licensed](https://img.shields.io/badge/license-MIT-blue.svg)](LICENSE) 2 | 3 | # nonlinear_control 4 | Implementing the nonlinear model predictive control, sliding mode control 5 | 6 | # Usage 7 | you can see the usage in each folder 8 | 9 | # Requirement 10 | You should install following software 11 | 12 | - python(3.5>) 13 | - numpy 14 | - matplotlib 15 | 16 | # License 17 | MIT 18 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Shunichi Sekiguchi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /nmpc/cgmres/main_example.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import math 4 | 5 | class SampleSystem(): 6 | """SampleSystem, this is the simulator 7 | Attributes 8 | ----------- 9 | x_1 : float 10 | system state 1 11 | x_2 : float 12 | system state 2 13 | history_x_1 : list 14 | time history of system state 1 (x_1) 15 | history_x_2 : list 16 | time history of system state 2 (x_2) 17 | """ 18 | def __init__(self, init_x_1=0., init_x_2=0.): 19 | """ 20 | Palameters 21 | ----------- 22 | init_x_1 : float, optional 23 | initial value of x_1, default is 0. 24 | init_x_2 : float, optional 25 | initial value of x_2, default is 0. 26 | """ 27 | self.x_1 = init_x_1 28 | self.x_2 = init_x_2 29 | self.history_x_1 = [init_x_1] 30 | self.history_x_2 = [init_x_2] 31 | 32 | def update_state(self, u, dt=0.01): 33 | """ 34 | Palameters 35 | ------------ 36 | u : float 37 | input of system in some cases this means the reference 38 | dt : float in seconds, optional 39 | sampling time of simulation, default is 0.01 [s] 40 | """ 41 | # for theta 1, theta 1 dot, theta 2, theta 2 dot 42 | k0 = [0.0 for _ in range(2)] 43 | k1 = [0.0 for _ in range(2)] 44 | k2 = [0.0 for _ in range(2)] 45 | k3 = [0.0 for _ in range(2)] 46 | 47 | functions = [self._func_x_1, self._func_x_2] 48 | 49 | # solve Runge-Kutta 50 | for i, func in enumerate(functions): 51 | k0[i] = dt * func(self.x_1, self.x_2, u) 52 | 53 | for i, func in enumerate(functions): 54 | k1[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., u) 55 | 56 | for i, func in enumerate(functions): 57 | k2[i] = dt * func(self.x_1 + k1[0]/2., self.x_2 + k1[1]/2., u) 58 | 59 | for i, func in enumerate(functions): 60 | k3[i] = dt * func(self.x_1 + k2[0], self.x_2 + k2[1], u) 61 | 62 | self.x_1 += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6. 63 | self.x_2 += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6. 64 | 65 | # save 66 | self.history_x_1.append(self.x_1) 67 | self.history_x_2.append(self.x_2) 68 | 69 | def _func_x_1(self, y_1, y_2, u): 70 | """ 71 | Parameters 72 | ------------ 73 | y_1 : float 74 | y_2 : float 75 | u : float 76 | system input 77 | """ 78 | y_dot = y_2 79 | return y_dot 80 | 81 | def _func_x_2(self, y_1, y_2, u): 82 | """ 83 | Parameters 84 | ------------ 85 | y_1 : float 86 | y_2 : float 87 | u : float 88 | system input 89 | """ 90 | y_dot = (1. - y_1**2 - y_2**2) * y_2 - y_1 + u 91 | return y_dot 92 | 93 | 94 | class NMPCSimulatorSystem(): 95 | """SimulatorSystem for nmpc, this is the simulator of nmpc 96 | the reason why I seperate the real simulator and nmpc's simulator is sometimes the modeling error, disturbance can include in real simulator 97 | Attributes 98 | ----------- 99 | None 100 | 101 | """ 102 | def __init__(self): 103 | """ 104 | Parameters 105 | ----------- 106 | None 107 | """ 108 | pass 109 | 110 | def calc_predict_and_adjoint_state(self, x_1, x_2, us, N, dt): 111 | """main 112 | Parameters 113 | ------------ 114 | x_1 : float 115 | current state 116 | x_2 : float 117 | current state 118 | us : list of float 119 | estimated optimal input Us for N steps 120 | N : int 121 | predict step 122 | dt : float 123 | sampling time 124 | 125 | Returns 126 | -------- 127 | x_1s : list of float 128 | predicted x_1s for N steps 129 | x_2s : list of float 130 | predicted x_2s for N steps 131 | lam_1s : list of float 132 | adjoint state of x_1s, lam_1s for N steps 133 | lam_2s : list of float 134 | adjoint state of x_2s, lam_2s for N steps 135 | """ 136 | 137 | x_1s, x_2s = self._calc_predict_states(x_1, x_2, us, N, dt) # by usin state equation 138 | lam_1s, lam_2s = self._calc_adjoint_states(x_1s, x_2s, us, N, dt) # by using adjoint equation 139 | 140 | return x_1s, x_2s, lam_1s, lam_2s 141 | 142 | def _calc_predict_states(self, x_1, x_2, us, N, dt): 143 | """ 144 | Parameters 145 | ------------ 146 | x_1 : float 147 | current state 148 | x_2 : float 149 | current state 150 | us : list of float 151 | estimated optimal input Us for N steps 152 | N : int 153 | predict step 154 | dt : float 155 | sampling time 156 | 157 | Returns 158 | -------- 159 | x_1s : list of float 160 | predicted x_1s for N steps 161 | x_2s : list of float 162 | predicted x_2s for N steps 163 | """ 164 | # initial state 165 | x_1s = [x_1] 166 | x_2s = [x_2] 167 | 168 | for i in range(N): 169 | temp_x_1, temp_x_2 = self._predict_state_with_oylar(x_1s[i], x_2s[i], us[i], dt) 170 | x_1s.append(temp_x_1) 171 | x_2s.append(temp_x_2) 172 | 173 | return x_1s, x_2s 174 | 175 | def _calc_adjoint_states(self, x_1s, x_2s, us, N, dt): 176 | """ 177 | Parameters 178 | ------------ 179 | x_1s : list of float 180 | predicted x_1s for N steps 181 | x_2s : list of float 182 | predicted x_2s for N steps 183 | us : list of float 184 | estimated optimal input Us for N steps 185 | N : int 186 | predict step 187 | dt : float 188 | sampling time 189 | 190 | Returns 191 | -------- 192 | lam_1s : list of float 193 | adjoint state of x_1s, lam_1s for N steps 194 | lam_2s : list of float 195 | adjoint state of x_2s, lam_2s for N steps 196 | """ 197 | # final state 198 | # final_state_func 199 | lam_1s = [x_1s[-1]] 200 | lam_2s = [x_2s[-1]] 201 | 202 | for i in range(N-1, 0, -1): 203 | temp_lam_1, temp_lam_2 = self._adjoint_state_with_oylar(x_1s[i], x_2s[i], lam_1s[0] ,lam_2s[0], us[i], dt) 204 | lam_1s.insert(0, temp_lam_1) 205 | lam_2s.insert(0, temp_lam_2) 206 | 207 | return lam_1s, lam_2s 208 | 209 | def final_state_func(self): 210 | """this func usually need 211 | """ 212 | pass 213 | 214 | def _predict_state_with_oylar(self, x_1, x_2, u, dt): 215 | """in this case this function is the same as simulator 216 | Parameters 217 | ------------ 218 | x_1 : float 219 | system state 220 | x_2 : float 221 | system state 222 | u : float 223 | system input 224 | dt : float in seconds 225 | sampling time 226 | Returns 227 | -------- 228 | next_x_1 : float 229 | next state, x_1 calculated by using state equation 230 | next_x_2 : float 231 | next state, x_2 calculated by using state equation 232 | """ 233 | k0 = [0. for _ in range(2)] 234 | 235 | functions = [self.func_x_1, self.func_x_2] 236 | 237 | for i, func in enumerate(functions): 238 | k0[i] = dt * func(x_1, x_2, u) 239 | 240 | next_x_1 = x_1 + k0[0] 241 | next_x_2 = x_2 + k0[1] 242 | 243 | return next_x_1, next_x_2 244 | 245 | def func_x_1(self, y_1, y_2, u): 246 | """calculating \dot{x_1} 247 | Parameters 248 | ------------ 249 | y_1 : float 250 | means x_1 251 | y_2 : float 252 | means x_2 253 | u : float 254 | means system input 255 | Returns 256 | --------- 257 | y_dot : float 258 | means \dot{x_1} 259 | """ 260 | y_dot = y_2 261 | return y_dot 262 | 263 | def func_x_2(self, y_1, y_2, u): 264 | """calculating \dot{x_2} 265 | Parameters 266 | ------------ 267 | y_1 : float 268 | means x_1 269 | y_2 : float 270 | means x_2 271 | u : float 272 | means system input 273 | Returns 274 | --------- 275 | y_dot : float 276 | means \dot{x_2} 277 | """ 278 | y_dot = (1. - y_1**2 - y_2**2) * y_2 - y_1 + u 279 | return y_dot 280 | 281 | def _adjoint_state_with_oylar(self, x_1, x_2, lam_1, lam_2, u, dt): 282 | """ 283 | Parameters 284 | ------------ 285 | x_1 : float 286 | system state 287 | x_2 : float 288 | system state 289 | lam_1 : float 290 | adjoint state 291 | lam_2 : float 292 | adjoint state 293 | u : float 294 | system input 295 | dt : float in seconds 296 | sampling time 297 | Returns 298 | -------- 299 | pre_lam_1 : float 300 | pre, 1 step before lam_1 calculated by using adjoint equation 301 | pre_lam_2 : float 302 | pre, 1 step before lam_2 calculated by using adjoint equation 303 | """ 304 | k0 = [0. for _ in range(2)] 305 | 306 | functions = [self._func_lam_1, self._func_lam_2] 307 | 308 | for i, func in enumerate(functions): 309 | k0[i] = dt * func(x_1, x_2, lam_1, lam_2, u) 310 | 311 | pre_lam_1 = lam_1 + k0[0] 312 | pre_lam_2 = lam_2 + k0[1] 313 | 314 | return pre_lam_1, pre_lam_2 315 | 316 | def _func_lam_1(self, y_1, y_2, y_3, y_4, u): 317 | """calculating -\dot{lam_1} 318 | Parameters 319 | ------------ 320 | y_1 : float 321 | means x_1 322 | y_2 : float 323 | means x_2 324 | y_3 : float 325 | means lam_1 326 | y_4 : float 327 | means lam_2 328 | u : float 329 | means system input 330 | Returns 331 | --------- 332 | y_dot : float 333 | means -\dot{lam_1} 334 | """ 335 | y_dot = y_1 - (2. * y_1 * y_2 + 1.) * y_4 336 | return y_dot 337 | 338 | def _func_lam_2(self, y_1, y_2, y_3, y_4, u): 339 | """calculating -\dot{lam_2} 340 | Parameters 341 | ------------ 342 | y_1 : float 343 | means x_1 344 | y_2 : float 345 | means x_2 346 | y_3 : float 347 | means lam_1 348 | y_4 : float 349 | means lam_2 350 | u : float 351 | means system input 352 | Returns 353 | --------- 354 | y_dot : float 355 | means -\dot{lam_2} 356 | """ 357 | y_dot = y_2 + y_3 + (-3. * (y_2**2) - y_1**2 + 1. ) * y_4 358 | return y_dot 359 | 360 | class NMPCController_with_CGMRES(): 361 | """ 362 | Attributes 363 | ------------ 364 | zeta : float 365 | gain of optimal answer stability 366 | ht : float 367 | update value of NMPC this should be decided by zeta 368 | tf : float 369 | predict time 370 | alpha : float 371 | gain of predict time 372 | N : int 373 | predicte step, discritize value 374 | threshold : float 375 | cgmres's threshold value 376 | input_num : int 377 | system input length, this should include dummy u and constraint variables 378 | max_iteration : int 379 | decide by the solved matrix size 380 | simulator : NMPCSimulatorSystem class 381 | us : list of float 382 | estimated optimal system input 383 | dummy_us : list of float 384 | estimated dummy input 385 | raws : list of float 386 | estimated constraint variable 387 | history_u : list of float 388 | time history of actual system input 389 | history_dummy_u : list of float 390 | time history of actual dummy u 391 | history_raw : list of float 392 | time history of actual raw 393 | history_f : list of float 394 | time history of error of optimal 395 | """ 396 | def __init__(self): 397 | """ 398 | Parameters 399 | ----------- 400 | None 401 | """ 402 | # parameters 403 | self.zeta = 100. # 安定化ゲイン 404 | self.ht = 0.01 # 差分近似の幅 405 | self.tf = 1. # 最終時間 406 | self.alpha = 0.5 # 時間の上昇ゲイン 407 | self.N = 10 # 分割数 408 | self.threshold = 0.001 # break値 409 | 410 | self.input_num = 3 # dummy, 制約条件に対するuにも合わせた入力の数 411 | self.max_iteration = self.input_num * self.N 412 | 413 | # simulator 414 | self.simulator = NMPCSimulatorSystem() 415 | 416 | # initial 417 | self.us = np.zeros(self.N) 418 | self.dummy_us = np.ones(self.N) * 0.49 419 | self.raws = np.ones(self.N) * 0.011 420 | 421 | # for fig 422 | self.history_u = [] 423 | self.history_dummy_u = [] 424 | self.history_raw = [] 425 | self.history_f = [] 426 | 427 | def calc_input(self, x_1, x_2, time): 428 | """ 429 | Parameters 430 | ------------ 431 | x_1 : float 432 | current state 433 | x_2 : float 434 | current state 435 | time : float in seconds 436 | now time 437 | Returns 438 | -------- 439 | us : list of float 440 | estimated optimal system input 441 | """ 442 | # calculating sampling time 443 | dt = self.tf * (1. - np.exp(-self.alpha * time)) / float(self.N) 444 | 445 | # x_dot 446 | x_1_dot = self.simulator.func_x_1(x_1, x_2, self.us[0]) 447 | x_2_dot = self.simulator.func_x_2(x_1, x_2, self.us[0]) 448 | 449 | dx_1 = x_1_dot * self.ht 450 | dx_2 = x_2_dot * self.ht 451 | 452 | x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, self.us, self.N, dt) 453 | 454 | # Fxt 455 | Fxt = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, 456 | self.raws, self.N, dt) 457 | 458 | # F 459 | x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, self.us, self.N, dt) 460 | 461 | F = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, 462 | self.raws, self.N, dt) 463 | 464 | right = -self.zeta * F - ((Fxt - F) / self.ht) 465 | 466 | du = self.us * self.ht 467 | ddummy_u = self.dummy_us * self.ht 468 | draw = self.raws * self.ht 469 | 470 | x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, self.us + du, self.N, dt) 471 | 472 | Fuxt = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us + du, self.dummy_us + ddummy_u, 473 | self.raws + draw, self.N, dt) 474 | 475 | left = ((Fuxt - Fxt) / self.ht) 476 | 477 | # calculationg cgmres 478 | r0 = right - left 479 | r0_norm = np.linalg.norm(r0) 480 | 481 | vs = np.zeros((self.max_iteration, self.max_iteration + 1)) # 数×iterarion回数 482 | 483 | vs[:, 0] = r0 / r0_norm # 最初の基底を算出 484 | 485 | hs = np.zeros((self.max_iteration + 1, self.max_iteration + 1)) 486 | 487 | e = np.zeros((self.max_iteration + 1, 1)) # in this case the state is 3(u and dummy_u) 488 | e[0] = 1. 489 | 490 | for i in range(self.max_iteration): 491 | du = vs[::3, i] * self.ht 492 | ddummy_u = vs[1::3, i] * self.ht 493 | draw = vs[2::3, i] * self.ht 494 | 495 | x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, self.us + du, self.N, dt) 496 | 497 | Fuxt = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us + du, self.dummy_us + ddummy_u, 498 | self.raws + draw, self.N, dt) 499 | 500 | Av = (( Fuxt - Fxt) / self.ht) 501 | 502 | sum_Av = np.zeros(self.max_iteration) 503 | 504 | for j in range(i + 1): # グラムシュミットの直交化法です、和を取って差分を取って算出します 505 | hs[j, i] = np.dot(Av, vs[:, j]) 506 | sum_Av = sum_Av + hs[j, i] * vs[:, j] 507 | 508 | v_est = Av - sum_Av 509 | 510 | hs[i+1, i] = np.linalg.norm(v_est) 511 | 512 | vs[:, i+1] = v_est / hs[i+1, i] 513 | 514 | inv_hs = np.linalg.pinv(hs[:i+1, :i]) # この辺は教科書（実時間の方）にのっています 515 | ys = np.dot(inv_hs, r0_norm * e[:i+1]) 516 | 517 | judge_value = r0_norm * e[:i+1] - np.dot(hs[:i+1, :i], ys[:i]) 518 | 519 | if np.linalg.norm(judge_value) < self.threshold or i == self.max_iteration-1: 520 | update_value = np.dot(vs[:, :i-1], ys_pre[:i-1]).flatten() 521 | du_new = du + update_value[::3] 522 | ddummy_u_new = ddummy_u + update_value[1::3] 523 | draw_new = draw + update_value[2::3] 524 | break 525 | 526 | ys_pre = ys 527 | 528 | # update 529 | self.us += du_new * self.ht 530 | self.dummy_us += ddummy_u_new * self.ht 531 | self.raws += draw_new * self.ht 532 | 533 | x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, self.us, self.N, dt) 534 | 535 | F = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, 536 | self.raws, self.N, dt) 537 | 538 | print("check F = {0}".format(np.linalg.norm(F))) 539 | 540 | # for save 541 | self.history_f.append(np.linalg.norm(F)) 542 | self.history_u.append(self.us[0]) 543 | self.history_dummy_u.append(self.dummy_us[0]) 544 | self.history_raw.append(self.raws[0]) 545 | 546 | return self.us 547 | 548 | def _calc_f(self, x_1s, x_2s, lam_1s, lam_2s, us, dummy_us, raws, N, dt): 549 | """ 550 | Parameters 551 | ------------ 552 | x_1s : list of float 553 | predicted x_1s for N steps 554 | x_2s : list of float 555 | predicted x_2s for N steps 556 | lam_1s : list of float 557 | adjoint state of x_1s, lam_1s for N steps 558 | lam_2s : list of float 559 | adjoint state of x_2s, lam_2s for N steps 560 | us : list of float 561 | estimated optimal system input 562 | dummy_us : list of float 563 | estimated dummy input 564 | raws : list of float 565 | estimated constraint variable 566 | N : int 567 | predict time step 568 | dt : float 569 | sampling time of system 570 | """ 571 | F = [] 572 | 573 | for i in range(N): 574 | F.append(us[i] + lam_2s[i] + 2. * raws[i] * us[i]) 575 | F.append(-0.01 + 2. * raws[i] * dummy_us[i]) 576 | F.append(us[i]**2 + dummy_us[i]**2 - 0.5**2) 577 | 578 | return np.array(F) 579 | 580 | def main(): 581 | # simulation time 582 | dt = 0.01 583 | iteration_time = 20. 584 | iteration_num = int(iteration_time/dt) 585 | 586 | # plant 587 | plant_system = SampleSystem(init_x_1=2., init_x_2=0.) 588 | 589 | # controller 590 | controller = NMPCController_with_CGMRES() 591 | 592 | # for i in range(iteration_num) 593 | for i in range(1, iteration_num): 594 | time = float(i) * dt 595 | x_1 = plant_system.x_1 596 | x_2 = plant_system.x_2 597 | # make input 598 | us = controller.calc_input(x_1, x_2, time) 599 | # update state 600 | plant_system.update_state(us[0]) 601 | 602 | # figure 603 | fig = plt.figure() 604 | 605 | x_1_fig = fig.add_subplot(321) 606 | x_2_fig = fig.add_subplot(322) 607 | u_fig = fig.add_subplot(323) 608 | dummy_fig = fig.add_subplot(324) 609 | raw_fig = fig.add_subplot(325) 610 | f_fig = fig.add_subplot(326) 611 | 612 | x_1_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_1) 613 | x_1_fig.set_xlabel("time [s]") 614 | x_1_fig.set_ylabel("x_1") 615 | 616 | x_2_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_2) 617 | x_2_fig.set_xlabel("time [s]") 618 | x_2_fig.set_ylabel("x_2") 619 | 620 | u_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_u) 621 | u_fig.set_xlabel("time [s]") 622 | u_fig.set_ylabel("u") 623 | 624 | dummy_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_dummy_u) 625 | dummy_fig.set_xlabel("time [s]") 626 | dummy_fig.set_ylabel("dummy u") 627 | 628 | raw_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_raw) 629 | raw_fig.set_xlabel("time [s]") 630 | raw_fig.set_ylabel("raw") 631 | 632 | f_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_f) 633 | f_fig.set_xlabel("time [s]") 634 | f_fig.set_ylabel("optimal error") 635 | 636 | fig.tight_layout() 637 | 638 | plt.show() 639 | 640 | 641 | if __name__ == "__main__": 642 | main() 643 | 644 | 645 | 646 | -------------------------------------------------------------------------------- /nmpc/cgmres/main_two_wheeled.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import math 4 | 5 | class TwoWheeledSystem(): 6 | """SampleSystem, this is the simulator 7 | Attributes 8 | ----------- 9 | x_1 : float 10 | system state 1 11 | x_2 : float 12 | system state 2 13 | history_x_1 : list 14 | time history of system state 1 (x_1) 15 | history_x_2 : list 16 | time history of system state 2 (x_2) 17 | """ 18 | def __init__(self, init_x_1=0., init_x_2=0., init_x_3=0.): 19 | """ 20 | Palameters 21 | ----------- 22 | init_x_1 : float, optional 23 | initial value of x_1, default is 0. 24 | init_x_2 : float, optional 25 | initial value of x_2, default is 0. 26 | init_x_3 : float, optional 27 | initial value of x_3, default is 0. 28 | """ 29 | self.x_1 = init_x_1 30 | self.x_2 = init_x_2 31 | self.x_3 = init_x_3 32 | self.history_x_1 = [init_x_1] 33 | self.history_x_2 = [init_x_2] 34 | self.history_x_3 = [init_x_3] 35 | 36 | def update_state(self, u_1, u_2, dt=0.01): 37 | """ 38 | Palameters 39 | ------------ 40 | u_1 : float 41 | input of system in some cases this means the reference, u_velocity 42 | u_2 : float 43 | input of system in some cases this means the reference, u_omega 44 | dt : float in seconds, optional 45 | sampling time of simulation, default is 0.01 [s] 46 | """ 47 | # for theta 1, theta 1 dot, theta 2, theta 2 dot 48 | k0 = [0.0 for _ in range(3)] 49 | k1 = [0.0 for _ in range(3)] 50 | k2 = [0.0 for _ in range(3)] 51 | k3 = [0.0 for _ in range(3)] 52 | 53 | functions = [self._func_x_1, self._func_x_2, self._func_x_3] 54 | 55 | # solve Runge-Kutta 56 | for i, func in enumerate(functions): 57 | k0[i] = dt * func(self.x_1, self.x_2, self.x_3, u_1, u_2) 58 | 59 | for i, func in enumerate(functions): 60 | k1[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., self.x_3 + k0[2]/2., u_1, u_2) 61 | 62 | for i, func in enumerate(functions): 63 | k2[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., self.x_3 + k0[2]/2., u_1, u_2) 64 | 65 | for i, func in enumerate(functions): 66 | k3[i] = dt * func(self.x_1 + k2[0], self.x_2 + k2[1], self.x_3 + k2[2], u_1, u_2) 67 | 68 | self.x_1 += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6. 69 | self.x_2 += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6. 70 | self.x_3 += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6. 71 | 72 | # save 73 | self.history_x_1.append(self.x_1) 74 | self.history_x_2.append(self.x_2) 75 | self.history_x_3.append(self.x_3) 76 | 77 | def _func_x_1(self, y_1, y_2, y_3, u_1, u_2): 78 | """ 79 | Parameters 80 | ------------ 81 | y_1 : float 82 | y_2 : float 83 | y_3 : float 84 | u_1 : float 85 | system input 86 | u_2 : float 87 | system input 88 | """ 89 | y_dot = math.cos(y_3) * u_1 90 | return y_dot 91 | 92 | def _func_x_2(self, y_1, y_2, y_3, u_1, u_2): 93 | """ 94 | Parameters 95 | ------------ 96 | y_1 : float 97 | y_2 : float 98 | y_3 : float 99 | u_1 : float 100 | system input 101 | u_2 : float 102 | system input 103 | """ 104 | y_dot = math.sin(y_3) * u_1 105 | return y_dot 106 | 107 | def _func_x_3(self, y_1, y_2, y_3, u_1, u_2): 108 | """ 109 | Parameters 110 | ------------ 111 | y_1 : float 112 | y_2 : float 113 | y_3 : float 114 | u_1 : float 115 | system input 116 | u_2 : float 117 | system input 118 | """ 119 | y_dot = u_2 120 | return y_dot 121 | 122 | class NMPCSimulatorSystem(): 123 | """SimulatorSystem for nmpc, this is the simulator of nmpc 124 | the reason why I seperate the real simulator and nmpc's simulator is sometimes the modeling error, disturbance can include in real simulator 125 | Attributes 126 | ----------- 127 | None 128 | 129 | """ 130 | def __init__(self): 131 | """ 132 | Parameters 133 | ----------- 134 | None 135 | """ 136 | pass 137 | 138 | def calc_predict_and_adjoint_state(self, x_1, x_2, x_3, u_1s, u_2s, N, dt): 139 | """main 140 | Parameters 141 | ------------ 142 | x_1 : float 143 | current state 144 | x_2 : float 145 | current state 146 | x_3 : float 147 | current state 148 | u_1s : list of float 149 | estimated optimal input Us for N steps 150 | u_2s : list of float 151 | estimated optimal input Us for N steps 152 | N : int 153 | predict step 154 | dt : float 155 | sampling time 156 | 157 | Returns 158 | -------- 159 | x_1s : list of float 160 | predicted x_1s for N steps 161 | x_2s : list of float 162 | predicted x_2s for N steps 163 | x_3s : list of float 164 | predicted x_3s for N steps 165 | lam_1s : list of float 166 | adjoint state of x_1s, lam_1s for N steps 167 | lam_2s : list of float 168 | adjoint state of x_2s, lam_2s for N steps 169 | lam_3s : list of float 170 | adjoint state of x_3s, lam_3s for N steps 171 | """ 172 | x_1s, x_2s, x_3s = self._calc_predict_states(x_1, x_2, x_3, u_1s, u_2s, N, dt) # by usin state equation 173 | lam_1s, lam_2s, lam_3s = self._calc_adjoint_states(x_1s, x_2s, x_3s, u_1s, u_2s, N, dt) # by using adjoint equation 174 | 175 | return x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s 176 | 177 | def _calc_predict_states(self, x_1, x_2, x_3, u_1s, u_2s, N, dt): 178 | """ 179 | Parameters 180 | ------------ 181 | x_1 : float 182 | current state 183 | x_2 : float 184 | current state 185 | x_3 : float 186 | current state 187 | u_1s : list of float 188 | estimated optimal input Us for N steps 189 | u_2s : list of float 190 | estimated optimal input Us for N steps 191 | N : int 192 | predict step 193 | dt : float 194 | sampling time 195 | 196 | Returns 197 | -------- 198 | x_1s : list of float 199 | predicted x_1s for N steps 200 | x_2s : list of float 201 | predicted x_2s for N steps 202 | x_3s : list of float 203 | predicted x_3s for N steps 204 | """ 205 | # initial state 206 | x_1s = [x_1] 207 | x_2s = [x_2] 208 | x_3s = [x_3] 209 | 210 | for i in range(N): 211 | temp_x_1, temp_x_2, temp_x_3 = self._predict_state_with_oylar(x_1s[i], x_2s[i], x_3s[i], u_1s[i], u_2s[i], dt) 212 | x_1s.append(temp_x_1) 213 | x_2s.append(temp_x_2) 214 | x_3s.append(temp_x_3) 215 | 216 | return x_1s, x_2s, x_3s 217 | 218 | def _calc_adjoint_states(self, x_1s, x_2s, x_3s, u_1s, u_2s, N, dt): 219 | """ 220 | Parameters 221 | ------------ 222 | x_1s : list of float 223 | predicted x_1s for N steps 224 | x_2s : list of float 225 | predicted x_2s for N steps 226 | x_3s : list of float 227 | predicted x_3s for N steps 228 | u_1s : list of float 229 | estimated optimal input Us for N steps 230 | u_2s : list of float 231 | estimated optimal input Us for N steps 232 | N : int 233 | predict step 234 | dt : float 235 | sampling time 236 | 237 | Returns 238 | -------- 239 | lam_1s : list of float 240 | adjoint state of x_1s, lam_1s for N steps 241 | lam_2s : list of float 242 | adjoint state of x_2s, lam_2s for N steps 243 | lam_3s : list of float 244 | adjoint state of x_2s, lam_2s for N steps 245 | """ 246 | # final state 247 | # final_state_func 248 | lam_1s = [x_1s[-1]] 249 | lam_2s = [x_2s[-1]] 250 | lam_3s = [x_3s[-1]] 251 | 252 | for i in range(N-1, 0, -1): 253 | temp_lam_1, temp_lam_2, temp_lam_3 = self._adjoint_state_with_oylar(x_1s[i], x_2s[i], x_3s[i], lam_1s[0] ,lam_2s[0], lam_3s[0], u_1s[i], u_2s[i], dt) 254 | lam_1s.insert(0, temp_lam_1) 255 | lam_2s.insert(0, temp_lam_2) 256 | lam_3s.insert(0, temp_lam_3) 257 | 258 | return lam_1s, lam_2s, lam_3s 259 | 260 | def final_state_func(self): 261 | """this func usually need 262 | """ 263 | pass 264 | 265 | def _predict_state_with_oylar(self, x_1, x_2, x_3, u_1, u_2, dt): 266 | """in this case this function is the same as simulator 267 | Parameters 268 | ------------ 269 | x_1 : float 270 | system state 271 | x_2 : float 272 | system state 273 | x_3 : float 274 | system state 275 | u_1 : float 276 | system input 277 | u_2 : float 278 | system input 279 | dt : float in seconds 280 | sampling time 281 | Returns 282 | -------- 283 | next_x_1 : float 284 | next state, x_1 calculated by using state equation 285 | next_x_2 : float 286 | next state, x_2 calculated by using state equation 287 | next_x_3 : float 288 | next state, x_3 calculated by using state equation 289 | """ 290 | k0 = [0. for _ in range(3)] 291 | 292 | functions = [self.func_x_1, self.func_x_2, self.func_x_3] 293 | 294 | for i, func in enumerate(functions): 295 | k0[i] = dt * func(x_1, x_2, x_3, u_1, u_2) 296 | 297 | next_x_1 = x_1 + k0[0] 298 | next_x_2 = x_2 + k0[1] 299 | next_x_3 = x_3 + k0[2] 300 | 301 | return next_x_1, next_x_2, next_x_3 302 | 303 | def func_x_1(self, y_1, y_2, y_3, u_1, u_2): 304 | """ 305 | Parameters 306 | ------------ 307 | y_1 : float 308 | y_2 : float 309 | y_3 : float 310 | u_1 : float 311 | system input 312 | u_2 : float 313 | system input 314 | """ 315 | y_dot = math.cos(y_3) * u_1 316 | return y_dot 317 | 318 | def func_x_2(self, y_1, y_2, y_3, u_1, u_2): 319 | """ 320 | Parameters 321 | ------------ 322 | y_1 : float 323 | y_2 : float 324 | y_3 : float 325 | u_1 : float 326 | system input 327 | u_2 : float 328 | system input 329 | """ 330 | y_dot = math.sin(y_3) * u_1 331 | return y_dot 332 | 333 | def func_x_3(self, y_1, y_2, y_3, u_1, u_2): 334 | """ 335 | Parameters 336 | ------------ 337 | y_1 : float 338 | y_2 : float 339 | y_3 : float 340 | u_1 : float 341 | system input 342 | u_2 : float 343 | system input 344 | """ 345 | y_dot = u_2 346 | return y_dot 347 | 348 | def _adjoint_state_with_oylar(self, x_1, x_2, x_3, lam_1, lam_2, lam_3, u_1, u_2, dt): 349 | """ 350 | Parameters 351 | ------------ 352 | x_1 : float 353 | system state 354 | x_2 : float 355 | system state 356 | x_3 : float 357 | system state 358 | lam_1 : float 359 | adjoint state 360 | lam_2 : float 361 | adjoint state 362 | lam_3 : float 363 | adjoint state 364 | u_1 : float 365 | system input 366 | u_2 : float 367 | system input 368 | dt : float in seconds 369 | sampling time 370 | Returns 371 | -------- 372 | pre_lam_1 : float 373 | pre, 1 step before lam_1 calculated by using adjoint equation 374 | pre_lam_2 : float 375 | pre, 1 step before lam_2 calculated by using adjoint equation 376 | pre_lam_3 : float 377 | pre, 1 step before lam_3 calculated by using adjoint equation 378 | """ 379 | k0 = [0. for _ in range(3)] 380 | 381 | functions = [self._func_lam_1, self._func_lam_2, self._func_lam_3] 382 | 383 | for i, func in enumerate(functions): 384 | k0[i] = dt * func(x_1, x_2, x_3, lam_1, lam_2, lam_3, u_1, u_2) 385 | 386 | pre_lam_1 = lam_1 + k0[0] 387 | pre_lam_2 = lam_2 + k0[1] 388 | pre_lam_3 = lam_3 + k0[2] 389 | 390 | return pre_lam_1, pre_lam_2, pre_lam_3 391 | 392 | def _func_lam_1(self, y_1, y_2, y_3, y_4, y_5, y_6, u_1, u_2): 393 | """calculating -\dot{lam_1} 394 | Parameters 395 | ------------ 396 | y_1 : float 397 | means x_1 398 | y_2 : float 399 | means x_2 400 | y_3 : float 401 | means x_3 402 | y_4 : float 403 | means lam_1 404 | y_5 : float 405 | means lam_2 406 | y_6 : float 407 | means lam_3 408 | u_1 : float 409 | means system input 410 | u_2 : float 411 | means system input 412 | Returns 413 | --------- 414 | y_dot : float 415 | means -\dot{lam_1} 416 | """ 417 | y_dot = 0. 418 | return y_dot 419 | 420 | def _func_lam_2(self, y_1, y_2, y_3, y_4, y_5, y_6, u_1, u_2): 421 | """calculating -\dot{lam_2} 422 | Parameters 423 | ------------ 424 | y_1 : float 425 | means x_1 426 | y_2 : float 427 | means x_2 428 | y_3 : float 429 | means x_3 430 | y_4 : float 431 | means lam_1 432 | y_5 : float 433 | means lam_2 434 | y_6 : float 435 | means lam_3 436 | u_1 : float 437 | means system input 438 | u_2 : float 439 | means system input 440 | Returns 441 | --------- 442 | y_dot : float 443 | means -\dot{lam_2} 444 | """ 445 | y_dot = 0. 446 | return y_dot 447 | 448 | def _func_lam_3(self, y_1, y_2, y_3, y_4, y_5, y_6, u_1, u_2): 449 | """calculating -\dot{lam_3} 450 | Parameters 451 | ------------ 452 | y_1 : float 453 | means x_1 454 | y_2 : float 455 | means x_2 456 | y_3 : float 457 | means x_3 458 | y_4 : float 459 | means lam_1 460 | y_5 : float 461 | means lam_2 462 | y_6 : float 463 | means lam_3 464 | u_1 : float 465 | means system input 466 | u_2 : float 467 | means system input 468 | Returns 469 | --------- 470 | y_dot : float 471 | means -\dot{lam_3} 472 | """ 473 | y_dot = - y_4 * math.sin(y_3) * u_1 + y_5 * math.cos(y_3) * u_1 474 | return y_dot 475 | 476 | class NMPCController_with_CGMRES(): 477 | """ 478 | Attributes 479 | ------------ 480 | zeta : float 481 | gain of optimal answer stability 482 | ht : float 483 | update value of NMPC this should be decided by zeta 484 | tf : float 485 | predict time 486 | alpha : float 487 | gain of predict time 488 | N : int 489 | predicte step, discritize value 490 | threshold : float 491 | cgmres's threshold value 492 | input_num : int 493 | system input length, this should include dummy u and constraint variables 494 | max_iteration : int 495 | decide by the solved matrix size 496 | simulator : NMPCSimulatorSystem class 497 | u_1s : list of float 498 | estimated optimal system input 499 | u_2s : list of float 500 | estimated optimal system input 501 | dummy_u_1s : list of float 502 | estimated dummy input 503 | dummy_u_2s : list of float 504 | estimated dummy input 505 | raw_1s : list of float 506 | estimated constraint variable 507 | raw_2s : list of float 508 | estimated constraint variable 509 | history_u_1 : list of float 510 | time history of actual system input 511 | history_u_2 : list of float 512 | time history of actual system input 513 | history_dummy_u_1 : list of float 514 | time history of actual dummy u_1 515 | history_dummy_u_2 : list of float 516 | time history of actual dummy u_2 517 | history_raw_1 : list of float 518 | time history of actual raw_1 519 | history_raw_2 : list of float 520 | time history of actual raw_2 521 | history_f : list of float 522 | time history of error of optimal 523 | """ 524 | def __init__(self): 525 | """ 526 | Parameters 527 | ----------- 528 | None 529 | """ 530 | # parameters 531 | self.zeta = 100. # 安定化ゲイン 532 | self.ht = 0.01 # 差分近似の幅 533 | self.tf = 1. # 最終時間 534 | self.alpha = 0.5 # 時間の上昇ゲイン 535 | self.N = 10 # 分割数 536 | self.threshold = 0.001 # break値 537 | 538 | self.input_num = 6 # dummy, 制約条件に対するuにも合わせた入力の数 539 | self.max_iteration = self.input_num * self.N 540 | 541 | # simulator 542 | self.simulator = NMPCSimulatorSystem() 543 | 544 | # initial 545 | self.u_1s = np.ones(self.N) * 1. 546 | self.u_2s = np.ones(self.N) * 0.1 547 | self.dummy_u_1s = np.ones(self.N) * 0.1 548 | self.dummy_u_2s = np.ones(self.N) * 2.5 549 | self.raw_1s = np.ones(self.N) * 0.8 550 | self.raw_2s = np.ones(self.N) * 0.8 551 | 552 | # for fig 553 | self.history_u_1 = [] 554 | self.history_u_2 = [] 555 | self.history_dummy_u_1 = [] 556 | self.history_dummy_u_2 = [] 557 | self.history_raw_1 = [] 558 | self.history_raw_2 = [] 559 | self.history_f = [] 560 | 561 | def calc_input(self, x_1, x_2, x_3, time): 562 | """ 563 | Parameters 564 | ------------ 565 | x_1 : float 566 | current state 567 | x_2 : float 568 | current state 569 | x_3 : float 570 | current state 571 | time : float in seconds 572 | now time 573 | Returns 574 | -------- 575 | u_1s : list of float 576 | estimated optimal system input 577 | u_2s : list of float 578 | estimated optimal system input 579 | """ 580 | # calculating sampling time 581 | dt = self.tf * (1. - np.exp(-self.alpha * time)) / float(self.N) 582 | 583 | # x_dot 584 | x_1_dot = self.simulator.func_x_1(x_1, x_2, x_3, self.u_1s[0], self.u_2s[0]) 585 | x_2_dot = self.simulator.func_x_2(x_1, x_2, x_3, self.u_1s[0], self.u_2s[0]) 586 | x_3_dot = self.simulator.func_x_3(x_1, x_2, x_3, self.u_1s[0], self.u_2s[0]) 587 | 588 | dx_1 = x_1_dot * self.ht 589 | dx_2 = x_2_dot * self.ht 590 | dx_3 = x_3_dot * self.ht 591 | 592 | x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, x_3 + dx_3, self.u_1s, self.u_2s, self.N, dt) 593 | 594 | # Fxt 595 | Fxt = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s, self.u_2s, self.dummy_u_1s, self.dummy_u_2s, 596 | self.raw_1s, self.raw_2s, self.N, dt) 597 | 598 | # F 599 | x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, x_3, self.u_1s, self.u_2s, self.N, dt) 600 | 601 | F = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s, self.u_2s, self.dummy_u_1s, self.dummy_u_2s, 602 | self.raw_1s, self.raw_2s, self.N, dt) 603 | 604 | right = -self.zeta * F - ((Fxt - F) / self.ht) 605 | 606 | du_1 = self.u_1s * self.ht 607 | du_2 = self.u_2s * self.ht 608 | ddummy_u_1 = self.dummy_u_1s * self.ht 609 | ddummy_u_2 = self.dummy_u_2s * self.ht 610 | draw_1 = self.raw_1s * self.ht 611 | draw_2 = self.raw_2s * self.ht 612 | 613 | x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, x_3 + dx_3, self.u_1s + du_1, self.u_2s + du_2, self.N, dt) 614 | 615 | Fuxt = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s + du_1, self.u_2s + du_2, self.dummy_u_1s + ddummy_u_1, self.dummy_u_2s + ddummy_u_2, 616 | self.raw_1s + draw_1, self.raw_2s + draw_2, self.N, dt) 617 | 618 | left = ((Fuxt - Fxt) / self.ht) 619 | 620 | # calculationg cgmres 621 | r0 = right - left 622 | r0_norm = np.linalg.norm(r0) 623 | 624 | vs = np.zeros((self.max_iteration, self.max_iteration + 1)) # 数×iterarion回数 625 | 626 | vs[:, 0] = r0 / r0_norm # 最初の基底を算出 627 | 628 | hs = np.zeros((self.max_iteration + 1, self.max_iteration + 1)) 629 | 630 | e = np.zeros((self.max_iteration + 1, 1)) # in this case the state is 3(u and dummy_u) 631 | e[0] = 1. 632 | 633 | for i in range(self.max_iteration): 634 | du_1 = vs[::self.input_num, i] * self.ht 635 | du_2 = vs[1::self.input_num, i] * self.ht 636 | ddummy_u_1 = vs[2::self.input_num, i] * self.ht 637 | ddummy_u_2 = vs[3::self.input_num, i] * self.ht 638 | draw_1 = vs[4::self.input_num, i] * self.ht 639 | draw_2 = vs[5::self.input_num, i] * self.ht 640 | 641 | x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, x_3 + dx_3, self.u_1s + du_1, self.u_2s + du_2, self.N, dt) 642 | 643 | Fuxt = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s + du_1, self.u_2s + du_2, self.dummy_u_1s + ddummy_u_1, self.dummy_u_2s + ddummy_u_2, 644 | self.raw_1s + draw_1, self.raw_2s + draw_2, self.N, dt) 645 | 646 | Av = (( Fuxt - Fxt) / self.ht) 647 | 648 | sum_Av = np.zeros(self.max_iteration) 649 | 650 | for j in range(i + 1): # グラムシュミットの直交化法です、和を取って差分を取って算出します 651 | hs[j, i] = np.dot(Av, vs[:, j]) 652 | sum_Av = sum_Av + hs[j, i] * vs[:, j] 653 | 654 | v_est = Av - sum_Av 655 | 656 | hs[i+1, i] = np.linalg.norm(v_est) 657 | 658 | vs[:, i+1] = v_est / hs[i+1, i] 659 | 660 | inv_hs = np.linalg.pinv(hs[:i+1, :i]) # この辺は教科書（実時間の方）にのっています 661 | ys = np.dot(inv_hs, r0_norm * e[:i+1]) 662 | 663 | judge_value = r0_norm * e[:i+1] - np.dot(hs[:i+1, :i], ys[:i]) 664 | 665 | if np.linalg.norm(judge_value) < self.threshold or i == self.max_iteration-1: 666 | update_value = np.dot(vs[:, :i-1], ys_pre[:i-1]).flatten() 667 | du_1_new = du_1 + update_value[::self.input_num] 668 | du_2_new = du_2 + update_value[1::self.input_num] 669 | ddummy_u_1_new = ddummy_u_1 + update_value[2::self.input_num] 670 | ddummy_u_2_new = ddummy_u_2 + update_value[3::self.input_num] 671 | draw_1_new = draw_1 + update_value[4::self.input_num] 672 | draw_2_new = draw_2 + update_value[5::self.input_num] 673 | break 674 | 675 | ys_pre = ys 676 | 677 | # update 678 | self.u_1s += du_1_new * self.ht 679 | self.u_2s += du_2_new * self.ht 680 | self.dummy_u_1s += ddummy_u_1_new * self.ht 681 | self.dummy_u_2s += ddummy_u_2_new * self.ht 682 | self.raw_1s += draw_1_new * self.ht 683 | self.raw_2s += draw_2_new * self.ht 684 | 685 | x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, x_3, self.u_1s, self.u_2s, self.N, dt) 686 | 687 | F = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s, self.u_2s, self.dummy_u_1s, self.dummy_u_2s, 688 | self.raw_1s, self.raw_2s, self.N, dt) 689 | 690 | print("check F = {0}".format(np.linalg.norm(F))) 691 | 692 | # for save 693 | self.history_f.append(np.linalg.norm(F)) 694 | self.history_u_1.append(self.u_1s[0]) 695 | self.history_u_2.append(self.u_2s[0]) 696 | self.history_dummy_u_1.append(self.dummy_u_1s[0]) 697 | self.history_dummy_u_2.append(self.dummy_u_2s[0]) 698 | self.history_raw_1.append(self.raw_1s[0]) 699 | self.history_raw_2.append(self.raw_2s[0]) 700 | 701 | return self.u_1s, self.u_2s 702 | 703 | def _calc_f(self, x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, u_1s, u_2s, dummy_u_1s, dummy_u_2s, raw_1s, raw_2s, N, dt): 704 | """ 705 | Parameters 706 | ------------ 707 | x_1s : list of float 708 | predicted x_1s for N steps 709 | x_2s : list of float 710 | predicted x_2s for N steps 711 | x_3s : list of float 712 | predicted x_3s for N steps 713 | lam_1s : list of float 714 | adjoint state of x_1s, lam_1s for N steps 715 | lam_2s : list of float 716 | adjoint state of x_2s, lam_2s for N steps 717 | lam_3s : list of float 718 | adjoint state of x_2s, lam_3s for N steps 719 | u_1s : list of float 720 | estimated optimal system input 721 | u_2s : list of float 722 | estimated optimal system input 723 | dummy_u_1s : list of float 724 | estimated dummy input 725 | dummy_u_2s : list of float 726 | estimated dummy input 727 | raw_1s : list of float 728 | estimated constraint variable 729 | raw_2s : list of float 730 | estimated constraint variable 731 | N : int 732 | predict time step 733 | dt : float 734 | sampling time of system 735 | """ 736 | F = [] 737 | 738 | for i in range(N): 739 | F.append(u_1s[i] + lam_1s[i] * math.cos(x_3s[i]) + lam_2s[i] * math.sin(x_3s[i]) + 2 * raw_1s[i] * u_1s[i]) 740 | F.append(u_2s[i] + lam_3s[i] + 2 * raw_2s[i] * u_2s[i]) 741 | F.append(-0.01 + 2. * raw_1s[i] * dummy_u_1s[i]) 742 | F.append(-0.01 + 2. * raw_2s[i] * dummy_u_2s[i]) 743 | F.append(u_1s[i]**2 + dummy_u_1s[i]**2 - 1.**2) 744 | F.append(u_2s[i]**2 + dummy_u_2s[i]**2 - 1.5**2) 745 | 746 | return np.array(F) 747 | 748 | def circle_make_with_angles(center_x, center_y, radius, angle): 749 | ''' 750 | Create circle matrix with angle line matrix 751 | 752 | Parameters 753 | ------- 754 | center_x : float 755 | the center x position of the circle 756 | center_y : float 757 | the center y position of the circle 758 | radius : float 759 | angle : float [rad] 760 | 761 | Returns 762 | ------- 763 | circle xs : numpy.ndarray 764 | circle ys : numpy.ndarray 765 | angle line xs : numpy.ndarray 766 | angle line ys : numpy.ndarray 767 | ''' 768 | 769 | point_num = 100 # 分解能 770 | 771 | circle_xs = [] 772 | circle_ys = [] 773 | 774 | for i in range(point_num + 1): 775 | circle_xs.append(center_x + radius * math.cos(i*2*math.pi/point_num)) 776 | circle_ys.append(center_y + radius * math.sin(i*2*math.pi/point_num)) 777 | 778 | angle_line_xs = [center_x, center_x + math.cos(angle) * radius] 779 | angle_line_ys = [center_y, center_y + math.sin(angle) * radius] 780 | 781 | return np.array(circle_xs), np.array(circle_ys), np.array(angle_line_xs), np.array(angle_line_ys) 782 | 783 | def main(): 784 | # simulation time 785 | dt = 0.01 786 | iteration_time = 15. 787 | iteration_num = int(iteration_time/dt) 788 | 789 | # plant 790 | plant_system = TwoWheeledSystem(init_x_1=-4.5, init_x_2=1.5, init_x_3=0.25) 791 | 792 | # controller 793 | controller = NMPCController_with_CGMRES() 794 | 795 | # for i in range(iteration_num) 796 | for i in range(1, iteration_num): 797 | time = float(i) * dt 798 | x_1 = plant_system.x_1 799 | x_2 = plant_system.x_2 800 | x_3 = plant_system.x_3 801 | # make input 802 | u_1s, u_2s = controller.calc_input(x_1, x_2, x_3, time) 803 | # update state 804 | plant_system.update_state(u_1s[0], u_2s[0]) 805 | 806 | # figure 807 | # time history 808 | fig_p = plt.figure() 809 | fig_u = plt.figure() 810 | fig_f = plt.figure() 811 | 812 | # traj 813 | fig_t = plt.figure() 814 | fig_traj = fig_t.add_subplot(111) 815 | fig_traj.set_aspect('equal') 816 | 817 | x_1_fig = fig_p.add_subplot(311) 818 | x_2_fig = fig_p.add_subplot(312) 819 | x_3_fig = fig_p.add_subplot(313) 820 | 821 | u_1_fig = fig_u.add_subplot(411) 822 | u_2_fig = fig_u.add_subplot(412) 823 | dummy_1_fig = fig_u.add_subplot(413) 824 | dummy_2_fig = fig_u.add_subplot(414) 825 | 826 | raw_1_fig = fig_f.add_subplot(311) 827 | raw_2_fig = fig_f.add_subplot(312) 828 | f_fig = fig_f.add_subplot(313) 829 | 830 | x_1_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_1) 831 | x_1_fig.set_xlabel("time [s]") 832 | x_1_fig.set_ylabel("x_1") 833 | 834 | x_2_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_2) 835 | x_2_fig.set_xlabel("time [s]") 836 | x_2_fig.set_ylabel("x_2") 837 | 838 | x_3_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_3) 839 | x_3_fig.set_xlabel("time [s]") 840 | x_3_fig.set_ylabel("x_3") 841 | 842 | u_1_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_u_1) 843 | u_1_fig.set_xlabel("time [s]") 844 | u_1_fig.set_ylabel("u_v") 845 | 846 | u_2_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_u_2) 847 | u_2_fig.set_xlabel("time [s]") 848 | u_2_fig.set_ylabel("u_omega") 849 | 850 | dummy_1_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_dummy_u_1) 851 | dummy_1_fig.set_xlabel("time [s]") 852 | dummy_1_fig.set_ylabel("dummy u_1") 853 | 854 | dummy_2_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_dummy_u_2) 855 | dummy_2_fig.set_xlabel("time [s]") 856 | dummy_2_fig.set_ylabel("dummy u_2") 857 | 858 | raw_1_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_raw_1) 859 | raw_1_fig.set_xlabel("time [s]") 860 | raw_1_fig.set_ylabel("raw_1") 861 | 862 | raw_2_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_raw_2) 863 | raw_2_fig.set_xlabel("time [s]") 864 | raw_2_fig.set_ylabel("raw_2") 865 | 866 | f_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_f) 867 | f_fig.set_xlabel("time [s]") 868 | f_fig.set_ylabel("optimal error") 869 | 870 | fig_traj.plot(plant_system.history_x_1, plant_system.history_x_2, color="b", linestyle="dashed") 871 | fig_traj.set_xlabel("x [m]") 872 | fig_traj.set_ylabel("y [m]") 873 | 874 | write_obj_num = 5 875 | count_num = int(iteration_num / write_obj_num) 876 | 877 | for i in np.arange(0, iteration_num, count_num): 878 | obj_xs, obj_ys, obj_line_xs, obj_line_ys = circle_make_with_angles(plant_system.history_x_1[i], plant_system.history_x_2[i], 0.5, plant_system.history_x_3[i]) 879 | fig_traj.plot(obj_xs, obj_ys, color="k") 880 | fig_traj.plot(obj_line_xs, obj_line_ys, color="k") 881 | 882 | fig_p.tight_layout() 883 | fig_u.tight_layout() 884 | fig_f.tight_layout() 885 | 886 | plt.show() 887 | 888 | 889 | if __name__ == "__main__": 890 | main() 891 | 892 | 893 | 894 | --------------------------------------------------------------------------------