|
| float | Ts = 0.05 |
| |
| int | N = 6 |
| |
| int | dim = 2 |
| |
| | model = HangingChain(N, dim) |
| |
| | f_d = model.dynamics(Ts) |
| |
| | y_null |
| |
| | u_null |
| |
| list | param = [0.03, 1.6, 0.033 / N] |
| |
| int | N_dist = 3 |
| |
| list | u_dist = 3 else [-0.5, 0.5] |
| |
| | y_dist = model.simulate(N_dist, y_null, u_dist, param) |
| |
| int | N_sim = 180 |
| |
| | y_sim = model.simulate(N_sim, y_dist[:, -1], u_null, param) |
| |
| int | N_horiz = 12 |
| |
| | L_cost = model.generate_cost_fun() |
| |
| | y_init = cs.SX.sym("y_init", *y_null.shape) |
| |
| | U = cs.SX.sym("U", dim * N_horiz) |
| |
| | constr_param = cs.SX.sym("c", 3) |
| |
| | mpc_param = cs.vertcat(y_init, model.params, constr_param) |
| |
| | U_mat = model.input_to_matrix(U) |
| |
| | mpc_sim = model.simulate(N_horiz, y_init, U_mat, model.params) |
| |
| int | mpc_cost = 0 |
| |
| | y_n = mpc_sim[:, n] |
| |
| | u_n = U_mat[:, n] |
| |
| | mpc_cost_fun = cs.Function('f_mpc', [U, mpc_param], [mpc_cost]) |
| |
| int | g_constr = lambda c, x: c[0] * x**3 + c[1] * x**2 + c[2] * x |
| |
| list | constr = [] |
| |
| | yx_n = y_n[dim * i] |
| |
| | yy_n = y_n[dim * i + dim - 1] |
| |
| | mpc_constr_fun = cs.Function("g", [U, mpc_param], [cs.vertcat(*constr)]) |
| |
| | a |
| |
| | b |
| |
| | c |
| |
| | d |
| |
| list | constr_coeff = [c, -3 * a * c, 3 * a * a * c + d] |
| |
| int | constr_lb = b - c * a**3 - d * a |
| |
| | prob = pa.generate_and_compile_casadi_problem(mpc_cost_fun, mpc_constr_fun) |
| |
| | lowerbound |
| |
| | upperbound |
| |
| | solver |
| |
| | n_state = y_n.shape[0] |
| |
| | y_mpc = np.empty((n_state, N_sim)) |
| |
| | controller = MPCController(model, prob) |
| |
| | fig |
| |
| | ax |
| |
| | x = np.linspace(-0.25, 1.25, 256) |
| |
| | y = np.linspace(-2.5, 1, 256) |
| |
| | z |
| |
| | label |
| |
| | X |
| |
| | Y |
| |
| int | Z = g_constr(constr_coeff, X) + constr_lb - Y |
| |
| list | fx = [patheffects.withTickedStroke(spacing=7, linewidth=0.8)] |
| |
| | cgc = plt.contour(X, Y, Z, [0], colors='tab:green', linewidths=0.8) |
| |
| | collections |
| |
| | path_effects |
| |
| | ani |
| |
| | out |
| |
| | exist_ok |
| |
