├── Makefile ├── .gitignore ├── eval_constraint_violation.py ├── eval_timings_fixed_nmass.py ├── README.md ├── eval_timings_vary_nmass.py ├── main.py ├── export_chain_mass_integrator.py ├── export_chain_mass_model.py ├── export_disturbed_chain_mass_model.py ├── minimal_example_sim.py ├── export_augmented_chain_mass_model.py ├── utils.py ├── run_nominal_control.py ├── toy_exampe.py ├── run_robust_control.py ├── run_fastzoro_robust_control.py ├── run_tailored_robust_control.py ├── plot_utils.py └── experiment_suboptimality.py /Makefile: -------------------------------------------------------------------------------- 1 | clean: 2 | rm -rf c_generated_code 3 | rm acados*.json -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ 2 | c_generated_code* 3 | .vscode 4 | *.json 5 | 6 | results 7 | figures 8 | -------------------------------------------------------------------------------- /eval_constraint_violation.py: -------------------------------------------------------------------------------- 1 | from utils import * 2 | import numpy as np 3 | from plot_utils import constraint_violation_box_plot 4 | 5 | chain_params = get_chain_params() 6 | 7 | IDs = ["nominal", "fastzoRO", "zoRO", "robust"] 8 | Seeds = range(1,30) 9 | n_mass = 3 10 | 11 | dist_dict = {id:[] for id in IDs} 12 | 13 | # load results 14 | for id in IDs: 15 | for seed in Seeds: 16 | chain_params["seed"] = seed 17 | chain_params["n_mass"] = n_mass 18 | results = load_results_from_json(id, chain_params) 19 | dist_dict[id] = dist_dict[id] + [min(results["wall_dist"])] 20 | 21 | # plot 22 | constraint_violation_box_plot(dist_dict, n_mass) 23 | 24 | -------------------------------------------------------------------------------- /eval_timings_fixed_nmass.py: -------------------------------------------------------------------------------- 1 | from utils import load_results_from_json, get_chain_params 2 | import numpy as np 3 | from plot_utils import timings_plot 4 | 5 | chain_params = get_chain_params() 6 | 7 | IDs = ["nominal", "fastzoRO", "zoRO", "robust"] 8 | Seeds = range(1, 2) 9 | 10 | for n_mass in [3, 6]: 11 | 12 | timings = {id:[] for id in IDs} 13 | 14 | # load results 15 | for id in IDs: 16 | for seed in Seeds: 17 | chain_params["seed"] = seed 18 | chain_params["n_mass"] = n_mass 19 | results = load_results_from_json(id, chain_params) 20 | total_timing = np.array(results["timings"]) + np.array(results["timings_P"]) 21 | timings[id] = timings[id] + list(total_timing) 22 | 23 | timings_plot(timings, n_mass) 24 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robust control of a nonlinear chain using acados Python 2 | This repository contains the code corresponding to the publication 3 | 4 | *Zero-Order Robust Nonlinear Model Predictive Control with Ellipsoidal Uncertainty Sets* 5 | 6 | by 7 | 8 | Andrea Zanelli, Jonathan Frey, Florian Messerer, Moritz Diehl 9 | 10 | # Update 2024: 11 | An efficient and customizable implementation of zoRO in `acados` is presented in: 12 | 13 | Efficient Zero-Order Robust Optimization for Real-Time Model Predictive Control with acados 14 | J. Frey, Y. Gao, F. Messerer, A. Lahr, M. N Zeilinger, M. Diehl 15 | Proceedings of the European Control Conference (ECC) (2024) 16 | 17 | https://publications.syscop.de/Frey2024.pdf 18 | 19 | 20 | ## References: 21 | https://github.com/dkouzoup/hanging-chain-acado 22 | -------------------------------------------------------------------------------- /eval_timings_vary_nmass.py: -------------------------------------------------------------------------------- 1 | from utils import * 2 | import numpy as np 3 | from plot_utils import timings_plot_vary_mass 4 | 5 | chain_params = get_chain_params() 6 | 7 | IDs = ["nominal", "fastzoRO", "zoRO", "robust"] 8 | Seeds = range(1,5) 9 | N_masses = range(3,7) 10 | 11 | # mass_dict = {nm: [] for nm in N_masses} 12 | timings = {id:dict() for id in IDs} 13 | 14 | # load results 15 | for id in IDs: 16 | for n_mass in N_masses: 17 | timings[id][n_mass] = [] 18 | for seed in Seeds: 19 | chain_params["seed"] = seed 20 | chain_params["n_mass"] = n_mass 21 | results = load_results_from_json(id, chain_params) 22 | total_timing = np.array(results["timings"]) + np.array(results["timings_P"]) 23 | timings[id][n_mass] = timings[id][n_mass] + list(total_timing) 24 | 25 | # plot 26 | timings_plot_vary_mass(timings, N_masses) 27 | 28 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | from run_nominal_control import run_nominal_control 2 | from run_robust_control import run_robust_control 3 | from run_tailored_robust_control import run_tailored_robust_control 4 | from run_fastzoro_robust_control import run_fastzoro_robust_control 5 | 6 | from utils import get_chain_params 7 | 8 | chain_params = get_chain_params() 9 | 10 | for n_mass in range(3, 7): 11 | for seed in range(1, 5): 12 | # adjust parameters wrt experiment 13 | chain_params["seed"] = seed 14 | chain_params["n_mass"] = n_mass 15 | 16 | # run all versions 17 | run_fastzoro_robust_control(chain_params) 18 | run_nominal_control(chain_params) 19 | run_robust_control(chain_params) 20 | run_tailored_robust_control(chain_params) 21 | 22 | 23 | # for n_mass in [5]: 24 | # for seed in range(10, 51): 25 | # # adjust parameters wrt experiment 26 | # chain_params["seed"] = seed 27 | # chain_params["n_mass"] = n_mass 28 | 29 | # # run all versions 30 | # run_nominal_control(chain_params) 31 | # run_robust_control(chain_params) 32 | # run_tailored_robust_control(chain_params) 33 | -------------------------------------------------------------------------------- /export_chain_mass_integrator.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy as np 3 | import scipy.linalg 4 | 5 | from acados_template import AcadosSim, AcadosOcpSolver, AcadosSimSolver 6 | 7 | from export_chain_mass_model import export_chain_mass_model 8 | from export_disturbed_chain_mass_model import export_disturbed_chain_mass_model 9 | 10 | from plot_utils import * 11 | from utils import * 12 | import matplotlib.pyplot as plt 13 | 14 | # create ocp object to formulate the simulation problem 15 | sim = AcadosSim() 16 | 17 | 18 | def export_chain_mass_integrator(n_mass, m, D, L): 19 | 20 | # simulation options 21 | Ts = 0.2 22 | 23 | # export model 24 | M = n_mass - 2 # number of intermediate masses 25 | model = export_disturbed_chain_mass_model(n_mass, m, D, L) 26 | 27 | # set model 28 | sim.model = model 29 | 30 | nx = model.x.size()[0] 31 | nu = model.u.size()[0] 32 | ny = nx + nu 33 | ny_e = nx 34 | 35 | # disturbances 36 | nparam = 3*M 37 | sim.parameter_values = np.zeros((nparam,)) 38 | 39 | # solver options 40 | sim.solver_options.integrator_type = 'IRK' 41 | 42 | sim.solver_options.sim_method_num_stages = 2 43 | sim.solver_options.sim_method_num_steps = 2 44 | # sim.solver_options.nlp_solver_tol_eq = 1e-9 45 | 46 | # set prediction horizon 47 | sim.solver_options.T = Ts 48 | 49 | # acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 50 | acados_integrator = AcadosSimSolver(sim, json_file = 'acados_ocp_' + model.name + '.json') 51 | 52 | return acados_integrator -------------------------------------------------------------------------------- /export_chain_mass_model.py: -------------------------------------------------------------------------------- 1 | from acados_template import AcadosModel 2 | from casadi import SX, vertcat, sin, cos, norm_2 3 | 4 | import numpy as np 5 | 6 | def export_chain_mass_model(n_mass, m, D, L): 7 | 8 | model_name = 'chain_mass_' + str(n_mass) 9 | x0 = np.array([0, 0, 0]) # fix mass (at wall) 10 | 11 | M = n_mass - 2 # number of intermediate masses 12 | 13 | nx = (2*M + 1)*3 # differential states 14 | nu = 3 # control inputs 15 | 16 | xpos = SX.sym('xpos', (M+1)*3, 1) # position of fix mass eliminated 17 | xvel = SX.sym('xvel', M*3, 1) 18 | u = SX.sym('u', nu, 1) 19 | xdot = SX.sym('xdot', nx, 1) 20 | 21 | f = SX.zeros(3*M, 1) # force on intermediate masses 22 | 23 | for i in range(M): 24 | f[3*i+2] = - 9.81 25 | 26 | for i in range(M+1): 27 | if i == 0: 28 | dist = xpos[i*3:(i+1)*3] - x0 29 | else: 30 | dist = xpos[i*3:(i+1)*3] - xpos[(i-1)*3:i*3] 31 | 32 | scale = D/m*(1-L/ norm_2(dist)) 33 | F = scale*dist 34 | 35 | # mass on the right 36 | if i < M: 37 | f[i*3:(i+1)*3] -= F 38 | 39 | # mass on the left 40 | if i > 0: 41 | f[(i-1)*3:i*3] += F 42 | 43 | # parameters 44 | p = [] 45 | 46 | x = vertcat(xpos, xvel) 47 | 48 | # dynamics 49 | f_expl = vertcat(xvel, u, f) 50 | f_impl = xdot - f_expl 51 | 52 | model = AcadosModel() 53 | 54 | model.f_impl_expr = f_impl 55 | model.f_expl_expr = f_expl 56 | model.x = x 57 | model.xdot = xdot 58 | model.u = u 59 | model.p = p 60 | model.name = model_name 61 | 62 | return model 63 | 64 | -------------------------------------------------------------------------------- /export_disturbed_chain_mass_model.py: -------------------------------------------------------------------------------- 1 | from acados_template import AcadosModel 2 | from casadi import SX, vertcat, sin, cos, norm_2 3 | 4 | import numpy as np 5 | 6 | def export_disturbed_chain_mass_model(n_mass, m, D, L): 7 | 8 | model_name = 'chain_mass_ds_' + str(n_mass) 9 | x0 = np.array([0, 0, 0]) # fix mass (at wall) 10 | 11 | M = n_mass - 2 # number of intermediate massesu 12 | 13 | nx = (2*M + 1)*3 # differential states 14 | nu = 3 # control inputs 15 | 16 | xpos = SX.sym('xpos', (M+1)*3, 1) # position of fix mass eliminated 17 | xvel = SX.sym('xvel', M*3, 1) 18 | u = SX.sym('u', nu, 1) 19 | xdot = SX.sym('xdot', nx, 1) 20 | w = SX.sym('w', M*3, 1) 21 | 22 | f = SX.zeros(3*M, 1) # force on intermediate masses 23 | 24 | for i in range(M): 25 | f[3*i+2] = - 9.81 26 | 27 | for i in range(M+1): 28 | if i == 0: 29 | dist = xpos[i*3:(i+1)*3] - x0 30 | else: 31 | dist = xpos[i*3:(i+1)*3] - xpos[(i-1)*3:i*3] 32 | 33 | scale = D/m*(1-L/ norm_2(dist)) 34 | F = scale*dist 35 | 36 | # mass on the right 37 | if i < M: 38 | f[i*3:(i+1)*3] -= F 39 | 40 | # mass on the left 41 | if i > 0: 42 | f[(i-1)*3:i*3] += F 43 | 44 | x = vertcat(xpos, xvel) 45 | 46 | # dynamics 47 | f_expl = vertcat(xvel, u, f+w) 48 | f_impl = xdot - f_expl 49 | 50 | model = AcadosModel() 51 | 52 | model.f_impl_expr = f_impl 53 | model.f_expl_expr = f_expl 54 | model.x = x 55 | model.xdot = xdot 56 | model.u = u 57 | model.p = w 58 | model.name = model_name 59 | 60 | return model 61 | 62 | -------------------------------------------------------------------------------- /minimal_example_sim.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy as np 3 | 4 | from acados_template import AcadosSim, AcadosSimSolver 5 | from export_chain_mass_model import export_chain_mass_model 6 | from utils import * 7 | from plot_utils import * 8 | 9 | 10 | sim = AcadosSim() 11 | 12 | # chain parameters 13 | chain_params = get_chain_params() 14 | 15 | n_mass = chain_params["n_mass"] 16 | M = chain_params["n_mass"] - 2 # number of intermediate masses 17 | # Ts = chain_params["Ts"] 18 | # Tsim = chain_params["Tsim"] 19 | N = chain_params["N"] 20 | # u_init = chain_params["u_init"] 21 | with_wall = chain_params["with_wall"] 22 | yPosWall = chain_params["yPosWall"] 23 | m = chain_params["m"] 24 | D = chain_params["D"] 25 | L = chain_params["L"] 26 | 27 | # export model 28 | model = export_chain_mass_model(n_mass, m, D, L) 29 | 30 | 31 | #%% simulate model 32 | sim.model = model 33 | 34 | Tf = 0.1 35 | nx = model.x.size()[0] 36 | nu = model.u.size()[0] 37 | N = 200 38 | 39 | # set simulation time 40 | sim.solver_options.T = Tf 41 | # set options 42 | sim.solver_options.num_stages = 4 43 | sim.solver_options.num_steps = 3 44 | sim.solver_options.newton_iter = 3 # for implicit integrator 45 | 46 | # create 47 | acados_integrator = AcadosSimSolver(sim) 48 | 49 | simX = np.ndarray((N+1, nx)) 50 | 51 | # position of last mass 52 | xPosFirstMass = np.zeros((3,1)) 53 | xEndRef = np.zeros((3,1)) 54 | xEndRef[0] = L * (M+1) * 6 55 | # initial state 56 | pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass) 57 | x0 = np.zeros((nx, 1)) 58 | x0[:3*(M+1):3] = pos0_x[1:].reshape((M+1,1)) 59 | 60 | 61 | u0 = np.zeros((nu, 1)) 62 | acados_integrator.set("u", u0) 63 | 64 | simX[0,:] = x0.flatten() 65 | 66 | for i in range(N): 67 | # set initial state 68 | acados_integrator.set("x", simX[i,:]) 69 | # solve 70 | status = acados_integrator.solve() 71 | # get solution 72 | simX[i+1,:] = acados_integrator.get("x") 73 | 74 | if status != 0: 75 | raise Exception('acados returned status {}. Exiting.'.format(status)) 76 | 77 | plot_chain_position_traj(simX) 78 | 79 | #%% find steady state 80 | 81 | xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef) 82 | 83 | plot_chain_position(xrest, xPosFirstMass) 84 | 85 | plot_chain_position_3D((xrest, simX[-1,:]), xPosFirstMass) 86 | 87 | ani = animate_chain_position_3D(simX, xPosFirstMass) 88 | ani = animate_chain_position(simX, xPosFirstMass) 89 | -------------------------------------------------------------------------------- /export_augmented_chain_mass_model.py: -------------------------------------------------------------------------------- 1 | from acados_template import AcadosModel 2 | from casadi import SX, vertcat, sin, cos, norm_2, Function, transpose, jacobian 3 | 4 | import numpy as np 5 | 6 | from export_disturbed_chain_mass_model import export_disturbed_chain_mass_model 7 | 8 | from utils import * 9 | 10 | def export_augmented_chain_mass_model(n_mass, m, D, L, h, W): 11 | 12 | model_name = 'chain_mass_aug_ds_' + str(n_mass) + '_ps_' + str(W[0][0]).replace('-','_').replace('.','o') 13 | og = export_disturbed_chain_mass_model(n_mass, m, D, L) 14 | nx = og.x.size()[0] 15 | 16 | aug_model = AcadosModel() 17 | 18 | P = SX.sym('P_vec', int((nx+1)*nx/2)) 19 | Pdot = SX.sym('Pdot_vec', int((nx+1)*nx/2)) 20 | 21 | x_aug = vertcat(og.x, P) 22 | 23 | A_fun = Function('A_fun', [og.x, og.u, og.p], [jacobian(og.f_expl_expr, og.x)]) 24 | A = A_fun(og.x, og.u, 0) 25 | 26 | # NOTE: need disturbances og.p here, but we eliminate the disturbances from the model later. 27 | B_fun = Function('B_fun', [og.x, og.u, og.p], [jacobian(og.f_expl_expr, og.p)]) 28 | B = B_fun(og.x, og.u, 0) 29 | 30 | P_mat = vec2sym_mat(P, nx) 31 | Pdot_mat = A @ P_mat + P_mat @ A.T + B @ W @ B.T 32 | Pdot_expr = sym_mat2vec(Pdot_mat) 33 | # set up discrete dynamics 34 | f_expl = Function('f_expl_chain', [og.x, og.u, og.p], [og.f_expl_expr]) 35 | nominal_f_expl_expr = f_expl(og.x, og.u, 0) 36 | # k1 = f_expl(og.x, og.u, 0) 37 | # k2 = f_expl(og.x + h * k1/2, og.u, 0) 38 | # k3 = f_expl(og.x + h * k2/2, og.u, 0) 39 | # k4 = f_expl(og.x + h * k3, og.u, 0) 40 | # xplus = og.x + h/6 * (k1 + 2*k2 + 2*k3 + k4) 41 | 42 | # A_fun = Function('A_fun', [og.x, og.u, og.p], [jacobian(xplus, og.x)]) 43 | # A = A_fun(og.x, og.u, 0) 44 | 45 | # # NOTE: need disturbances og.p here, but we eliminate the disturbances from the model later. 46 | # B_fun = Function('B_fun', [og.x, og.u, og.p], [jacobian(xplus, og.p)]) 47 | # B = B_fun(og.x, og.u, 0) 48 | 49 | # P_mat = vec2sym_mat(P, nx) 50 | # P_mat = SX.zeros(nx,nx) 51 | # start_P = 0 52 | # for i in range(nx): 53 | # end_P = start_P + (nx - i) 54 | # P_mat[i,i:] = transpose(P[start_P:end_P]) 55 | # P_mat[i:,i] = P[start_P:end_P] 56 | # start_P += (nx-i) 57 | 58 | # Pplus_mat = A @ P_mat @ transpose(A) + B @ W @ transpose(B) 59 | 60 | # Pplus = SX.zeros(int((nx+1)*nx/2), 1) 61 | 62 | # start_P = 0 63 | # for i in range(nx): 64 | # end_P = start_P + (nx - i) 65 | # # P_mat[i,i:] = transpose(P[start_P:end_P]) 66 | # # P_mat[i:,i] = P[start_P:end_P] 67 | # Pplus[start_P:end_P] = Pplus_mat[i:,i] 68 | # start_P += (nx-i) 69 | 70 | # # euler trick 71 | # xplus_aug = vertcat(xplus, Pplus) 72 | # f_cont = (xplus_aug - x_aug) / h 73 | 74 | 75 | # fill model 76 | xdot = vertcat(og.xdot, Pdot) 77 | f_expl_expr = vertcat(nominal_f_expl_expr, Pdot_expr) 78 | aug_model.f_expl_expr = f_expl_expr 79 | aug_model.f_impl_expr = f_expl_expr - xdot 80 | 81 | aug_model.xdot = xdot 82 | aug_model.u = og.u 83 | aug_model.x = x_aug 84 | aug_model.name = model_name 85 | 86 | return aug_model 87 | 88 | -------------------------------------------------------------------------------- /utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import json 3 | import numpy as np 4 | import casadi as ca 5 | from export_chain_mass_model import export_chain_mass_model 6 | 7 | 8 | def get_chain_params(): 9 | params = dict() 10 | 11 | params["n_mass"] = 5 12 | params["Ts"] = 0.2 13 | params["Tsim"] = 5 14 | params["N"] = 40 15 | params["u_init"] = np.array([-1, 1, 1]) 16 | params["with_wall"] = True 17 | params["yPosWall"] = -0.05 # Dimitris: - 0.1; 18 | params["m"] = 0.033 # mass of the balls 19 | params["D"] = 1.0 # spring constant 20 | params["L"] = 0.033 # rest length of spring 21 | params["perturb_scale"] = 1e-2 22 | 23 | params["save_results"] = True 24 | params["show_plots"] = False 25 | params["nlp_iter"] = 50 26 | params["seed"] = 50 27 | params["nlp_tol"] = 1e-5 28 | 29 | return params 30 | 31 | 32 | 33 | def compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef): 34 | 35 | model = export_chain_mass_model(n_mass, m, D, L) 36 | nx = model.x.shape[0] 37 | M = int((nx/3 -1)/2) 38 | 39 | # initial guess for state 40 | pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass) 41 | x0 = np.zeros((nx, 1)) 42 | x0[:3*(M+1):3] = pos0_x[1:].reshape((M+1,1)) 43 | 44 | # decision variables 45 | w = [model.x, model.xdot, model.u] 46 | # initial guess 47 | w0 = ca.vertcat(*[x0, np.zeros(model.xdot.shape), np.zeros(model.u.shape)]) 48 | 49 | # constraints 50 | g = [] 51 | g += [model.f_impl_expr] # steady state 52 | g += [model.x[3*M:3*(M+1)] - xEndRef] # fix position of last mass 53 | g += [model.u] # don't actuate controlled mass 54 | 55 | # misuse IPOPT as nonlinear equation solver 56 | nlp = {'x': ca.vertcat(*w), 'f': 0, 'g': ca.vertcat(*g)} 57 | 58 | solver = ca.nlpsol('solver', 'ipopt', nlp) 59 | sol = solver(x0=w0,lbg=0,ubg=0) 60 | 61 | wrest = sol['x'].full() 62 | xrest = wrest[:nx] 63 | 64 | return xrest 65 | 66 | 67 | def sampleFromEllipsoid(w, Z): 68 | """ 69 | draws uniform(?) sample from ellipsoid with center w and variability matrix Z 70 | """ 71 | 72 | n = w.shape[0] # dimension 73 | lam, v = np.linalg.eig(Z) 74 | 75 | # sample in hypersphere 76 | r = np.random.rand()**(1/n) # radial position of sample 77 | x = np.random.randn(n) 78 | x = x / np.linalg.norm(x) 79 | x *= r 80 | # project to ellipsoid 81 | y = v @ (np.sqrt(lam) * x) + w 82 | 83 | return y 84 | 85 | 86 | def sym_mat2vec(mat): 87 | nx = mat.shape[0] 88 | 89 | if isinstance(mat, np.ndarray): 90 | vec = np.zeros((int((nx+1)*nx/2),)) 91 | else: 92 | vec = ca.SX.zeros(int((nx+1)*nx/2)) 93 | 94 | start_mat = 0 95 | for i in range(nx): 96 | end_mat = start_mat + (nx - i) 97 | vec[start_mat:end_mat] = mat[i:,i] 98 | start_mat += (nx-i) 99 | 100 | return vec 101 | 102 | 103 | def vec2sym_mat(vec, nx): 104 | # nx = (vec.shape[0]) 105 | 106 | if isinstance(vec, np.ndarray): 107 | mat = np.zeros((nx,nx)) 108 | else: 109 | mat = ca.SX.zeros(nx,nx) 110 | 111 | start_mat = 0 112 | for i in range(nx): 113 | end_mat = start_mat + (nx - i) 114 | aux = vec[start_mat:end_mat] 115 | mat[i,i:] = aux.T 116 | mat[i:,i] = aux 117 | start_mat += (nx-i) 118 | 119 | return mat 120 | 121 | 122 | def is_pos_def(mat): 123 | try: 124 | np.linalg.cholesky(mat) 125 | return 1 126 | except np.linalg.linalg.LinAlgError as err: 127 | if 'Matrix is not positive definite' in err.args[0]: 128 | return 0 129 | else: 130 | raise 131 | 132 | 133 | def P_propagation(P, A, B, W): 134 | # P_i+1 = A P A^T + B*W*B^T 135 | return A @ P @ A.T + B @ W @ B.T 136 | 137 | 138 | def np_array_to_list(np_array): 139 | if isinstance(np_array, (np.ndarray)): 140 | return np_array.tolist() 141 | elif isinstance(np_array, (SX)): 142 | return DM(np_array).full() 143 | elif isinstance(np_array, (DM)): 144 | return np_array.full() 145 | else: 146 | raise(Exception( 147 | "Cannot convert to list type {}".format(type(np_array)) 148 | )) 149 | 150 | 151 | def save_results_as_json(result_dict, json_file): 152 | 153 | with open(json_file, 'w') as f: 154 | json.dump(result_dict, f, default=np_array_to_list, indent=4, sort_keys=True) 155 | 156 | return 157 | 158 | 159 | def save_closed_loop_results_as_json(ID, timings, timings_P, wall_dist, chain_params): 160 | 161 | result_dict = dict() 162 | 163 | result_dict["timings"] = timings 164 | result_dict["wall_dist"] = wall_dist 165 | result_dict["timings_P"] = timings_P 166 | result_dict["chain_params"] = chain_params 167 | 168 | if not os.path.isdir('results'): 169 | os.makedirs('results') 170 | 171 | json_file = os.path.join('results', ID + '_nm_' + str(chain_params["n_mass"]) + \ 172 | '_seed_' + str(chain_params["seed"]) + '_iter_' + str(chain_params["nlp_iter"]) + '.json') 173 | 174 | save_results_as_json(result_dict, json_file) 175 | 176 | return 177 | 178 | 179 | 180 | def load_results_from_json(ID, chain_params): 181 | 182 | if not os.path.isdir('results'): 183 | os.makedirs('results') 184 | 185 | json_file = os.path.join('results', ID + '_nm_' + str(chain_params["n_mass"]) + \ 186 | '_seed_' + str(chain_params["seed"]) + '_iter_' + str(chain_params["nlp_iter"]) + '.json') 187 | 188 | with open(json_file, 'r') as f: 189 | results = json.load(f) 190 | 191 | return results 192 | -------------------------------------------------------------------------------- /run_nominal_control.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy as np 3 | import scipy.linalg 4 | 5 | from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver 6 | 7 | from export_chain_mass_model import export_chain_mass_model 8 | from export_disturbed_chain_mass_model import export_disturbed_chain_mass_model 9 | from export_chain_mass_integrator import export_chain_mass_integrator 10 | 11 | from plot_utils import * 12 | from utils import * 13 | import matplotlib.pyplot as plt 14 | 15 | def run_nominal_control(chain_params): 16 | # create ocp object to formulate the OCP 17 | ocp = AcadosOcp() 18 | 19 | # chain parameters 20 | n_mass = chain_params["n_mass"] 21 | M = chain_params["n_mass"] - 2 # number of intermediate masses 22 | Ts = chain_params["Ts"] 23 | Tsim = chain_params["Tsim"] 24 | N = chain_params["N"] 25 | u_init = chain_params["u_init"] 26 | with_wall = chain_params["with_wall"] 27 | yPosWall = chain_params["yPosWall"] 28 | m = chain_params["m"] 29 | D = chain_params["D"] 30 | L = chain_params["L"] 31 | perturb_scale = chain_params["perturb_scale"] 32 | 33 | nlp_iter = chain_params["nlp_iter"] 34 | nlp_tol = chain_params["nlp_tol"] 35 | save_results = chain_params["save_results"] 36 | show_plots = chain_params["show_plots"] 37 | seed = chain_params["seed"] 38 | 39 | np.random.seed(seed) 40 | 41 | nparam = 3*M 42 | W = perturb_scale * np.eye(nparam) 43 | 44 | # export model 45 | model = export_disturbed_chain_mass_model(n_mass, m, D, L) 46 | 47 | # set model 48 | ocp.model = model 49 | 50 | nx = model.x.size()[0] 51 | nu = model.u.size()[0] 52 | ny = nx + nu 53 | ny_e = nx 54 | Tf = N * Ts 55 | 56 | # initial state 57 | xPosFirstMass = np.zeros((3,1)) 58 | xEndRef = np.zeros((3,1)) 59 | xEndRef[0] = L * (M+1) * 6 60 | pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass) 61 | 62 | xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef) 63 | 64 | x0 = xrest 65 | 66 | # set dimensions 67 | ocp.solver_options.N_horizon = N 68 | 69 | # set cost module 70 | ocp.cost.cost_type = 'LINEAR_LS' 71 | ocp.cost.cost_type_e = 'LINEAR_LS' 72 | 73 | Q = 2*np.diagflat( np.ones((nx, 1)) ) 74 | q_diag = np.ones((nx,1)) 75 | strong_penalty = M+1 76 | q_diag[3*M] = strong_penalty 77 | q_diag[3*M+1] = strong_penalty 78 | q_diag[3*M+2] = strong_penalty 79 | Q = 2*np.diagflat( q_diag ) 80 | 81 | R = 2*np.diagflat( 1e-2 * np.ones((nu, 1)) ) 82 | 83 | ocp.cost.W = scipy.linalg.block_diag(Q, R) 84 | ocp.cost.W_e = Q 85 | 86 | ocp.cost.Vx = np.zeros((ny, nx)) 87 | ocp.cost.Vx[:nx,:nx] = np.eye(nx) 88 | 89 | Vu = np.zeros((ny, nu)) 90 | Vu[nx:nx+nu, :] = np.eye(nu) 91 | ocp.cost.Vu = Vu 92 | 93 | ocp.cost.Vx_e = np.eye(nx) 94 | 95 | # import pdb; pdb.set_trace() 96 | yref = np.vstack((xrest, np.zeros((nu,1)))).flatten() 97 | ocp.cost.yref = yref 98 | ocp.cost.yref_e = xrest.flatten() 99 | 100 | # set constraints 101 | umax = 1*np.ones((nu,)) 102 | 103 | ocp.constraints.constr_type = 'BGH' 104 | ocp.constraints.lbu = -umax 105 | ocp.constraints.ubu = umax 106 | ocp.constraints.x0 = x0.reshape((nx,)) 107 | ocp.constraints.idxbu = np.array(range(nu)) 108 | 109 | # disturbances 110 | nparam = 3*M 111 | ocp.parameter_values = np.zeros((nparam,)) 112 | 113 | # wall constraint 114 | if with_wall: 115 | nbx = M + 1 116 | Jbx = np.zeros((nbx,nx)) 117 | for i in range(nbx): 118 | Jbx[i, 3*i+1] = 1.0 119 | 120 | ocp.constraints.Jbx = Jbx 121 | ocp.constraints.lbx = yPosWall * np.ones((nbx,)) 122 | ocp.constraints.ubx = 1e9 * np.ones((nbx,)) 123 | 124 | # slacks 125 | ocp.constraints.Jsbx = np.eye(nbx) 126 | L2_pen = 1e3 127 | L1_pen = 1 128 | ocp.cost.Zl = L2_pen * np.ones((nbx,)) 129 | ocp.cost.Zu = L2_pen * np.ones((nbx,)) 130 | ocp.cost.zl = L1_pen * np.ones((nbx,)) 131 | ocp.cost.zu = L1_pen * np.ones((nbx,)) 132 | 133 | 134 | # solver options 135 | ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES 136 | ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' 137 | ocp.solver_options.integrator_type = 'IRK' 138 | ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI 139 | ocp.solver_options.nlp_solver_max_iter = nlp_iter 140 | 141 | ocp.solver_options.sim_method_num_stages = 2 142 | ocp.solver_options.sim_method_num_steps = 2 143 | ocp.solver_options.qp_solver_cond_N = N 144 | ocp.solver_options.qp_tol = nlp_tol 145 | ocp.solver_options.tol = nlp_tol 146 | # ocp.solver_options.nlp_solver_tol_eq = 1e-9 147 | 148 | # set prediction horizon 149 | ocp.solver_options.tf = Tf 150 | 151 | acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 152 | 153 | # acados_integrator = AcadosSimSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 154 | acados_integrator = export_chain_mass_integrator(n_mass, m, D, L) 155 | 156 | #%% get initial state from xrest 157 | xcurrent = x0.reshape((nx,)) 158 | for i in range(5): 159 | acados_integrator.set("x", xcurrent) 160 | acados_integrator.set("u", u_init) 161 | 162 | status = acados_integrator.solve() 163 | if status != 0: 164 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 165 | 166 | # update state 167 | xcurrent = acados_integrator.get("x") 168 | 169 | #%% actual simulation 170 | N_sim = int(np.floor(Tsim/Ts)) 171 | simX = np.ndarray((N_sim+1, nx)) 172 | simU = np.ndarray((N_sim, nu)) 173 | wall_dist = np.zeros((N_sim,)) 174 | 175 | timings = np.zeros((N_sim,)) 176 | 177 | simX[0,:] = xcurrent 178 | 179 | # closed loop 180 | for i in range(N_sim): 181 | 182 | # solve ocp 183 | acados_ocp_solver.set(0, "lbx", xcurrent) 184 | acados_ocp_solver.set(0, "ubx", xcurrent) 185 | 186 | status = acados_ocp_solver.solve() 187 | timings[i] = acados_ocp_solver.get_stats("time_tot")[0] 188 | 189 | if status != 0: 190 | raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i)) 191 | 192 | simU[i,:] = acados_ocp_solver.get(0, "u") 193 | print("control at time", i, ":", simU[i,:]) 194 | 195 | # simulate system 196 | acados_integrator.set("x", xcurrent) 197 | acados_integrator.set("u", simU[i,:]) 198 | 199 | pertubation = sampleFromEllipsoid(np.zeros((nparam,)), W) 200 | acados_integrator.set("p", pertubation) 201 | 202 | status = acados_integrator.solve() 203 | if status != 0: 204 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 205 | 206 | # update state 207 | xcurrent = acados_integrator.get("x") 208 | simX[i+1,:] = xcurrent 209 | 210 | # xOcpPredict = acados_ocp_solver.get(1, "x") 211 | # print("model mismatch = ", str(np.max(xOcpPredict - xcurrent))) 212 | yPos = xcurrent[range(1,3*M+1,3)] 213 | wall_dist[i] = np.min(yPos - yPosWall) 214 | print("time i = ", str(i), " dist2wall ", str(wall_dist[i])) 215 | 216 | print("dist2wall (minimum over simulation) ", str(np.min(wall_dist))) 217 | 218 | #%% plot results 219 | if show_plots: 220 | plot_chain_control_traj(simU) 221 | plot_chain_position_traj(simX, yPosWall=yPosWall) 222 | plot_chain_velocity_traj(simX) 223 | # plot_chain_position(simX[-1,:], xPosFirstMass) 224 | animate_chain_position(simX, xPosFirstMass, yPosWall=yPosWall) 225 | 226 | plt.show() 227 | 228 | #%% save results 229 | if save_results: 230 | ID = "nominal" 231 | timings_Pprop = np.zeros((N_sim,)) 232 | save_closed_loop_results_as_json(ID, timings, timings_Pprop, wall_dist, chain_params) 233 | -------------------------------------------------------------------------------- /toy_exampe.py: -------------------------------------------------------------------------------- 1 | import casadi as ca 2 | import numpy as np 3 | from scipy.linalg import sqrtm 4 | import matplotlib.pyplot as plt 5 | 6 | # UPDATE_FIG = True 7 | UPDATE_FIG = False 8 | 9 | plt.rcParams['text.usetex'] = True 10 | plt.rcParams['text.latex.preamble'] = [r'\usepackage{lmodern}'] 11 | font = {'family':'serif'} 12 | plt.rc('font',**font) 13 | 14 | maxiter = 500 15 | TOL = 1e-8 16 | 17 | # dims 18 | nx = 2 19 | nw = 2 20 | 21 | # uncertainty values 22 | sigma_v = np.linspace(1e-6,np.sqrt(5)*1.5,30) 23 | 24 | # variables 25 | x = ca.SX.sym('x', nx, 1) 26 | w = ca.SX.sym('x', nw, 1) 27 | P = ca.SX.sym('P', nw**2, 1) 28 | sigma = ca.SX.sym('sigma', 1, 1) 29 | 30 | # constants 31 | c = np.array([[0.0], [0.0]]) 32 | M = np.eye(2) 33 | 34 | # objective 35 | f = 1/2*ca.mtimes(ca.mtimes(x.T, M), x) 36 | # for plotting 37 | f_fun = ca.Function('f_fun', [x[0],x[1]], [f]) 38 | 39 | A = ca.SX.zeros(nw**2, nw**2) 40 | A[0,0] = 1 + 0.6*ca.sin(x[0]) 41 | A[1,1] = 1 + 0.6*ca.cos(x[1]) 42 | A[2,2] = 1 + 0.6*ca.sin(x[1]**2) 43 | A[3,3] = 1 44 | A[2,1] = 0.1 + ca.sin(x[1]) 45 | 46 | b = np.array([1,0,0,1]) 47 | 48 | # constraints 49 | h = -ca.sqrt((x[0] + w[0] -1)**2 + (x[1] + w[1] -1)**2) + ca.sqrt(10) 50 | 51 | h_w = ca.substitute(ca.jacobian(h, w), w, 0).T 52 | # for plotting 53 | h_fun = ca.Function('h_fun', [x[0],x[1]], [ca.substitute(h,w,0)]) 54 | 55 | P_ = P.reshape((nw,nw)) 56 | h_hat = ca.substitute(h,w,0) + ca.sqrt(ca.mtimes(ca.mtimes(h_w.T, P_), h_w)) 57 | 58 | g = ca.vertcat(ca.mtimes(A,P) - sigma**2*b, h_hat) 59 | 60 | ubg = np.zeros((nw**2+1,1)) 61 | lbg = np.zeros((nw**2+1,1)) 62 | lbg[-1] = -np.inf 63 | x0 = ca.vertcat(10*np.ones((nx,1)), 0.1*np.ones((nw**2,1))) 64 | 65 | # create nlp solver 66 | nlp = {'x':ca.vertcat(x,P), 'f':f, 'g':g, 'p':sigma} 67 | nlp_solver = ca.nlpsol("solver", "ipopt", nlp) 68 | 69 | # solve 70 | sol_x_v = [] 71 | for i in range(len(sigma_v)): 72 | sol = nlp_solver(x0=x0, lbg=lbg, ubg=ubg, p = sigma_v[i]) 73 | sol_x_v.append(sol['x']) 74 | x0 = sol['x'] 75 | status = nlp_solver.stats()['success'] 76 | if status != True: 77 | raise Exception('Solver failed at iteration {}'.format(i)) 78 | 79 | # define linearization points 80 | x_bar = ca.SX.sym('x_bar', nx, 1) 81 | P_bar = ca.SX.sym('P_bar', nw**2, 1) 82 | 83 | # define QP opt. variables 84 | dx = ca.SX.sym('dx', nx, 1) 85 | 86 | A_fun = ca.Function('A_bar', [x],[A]) 87 | b_fun = ca.Function('b_bar', [x],[b]) 88 | h_hat_fun = ca.Function('h_hat_bar', [x,P], [h_hat]) 89 | 90 | # define QP 91 | h_hat_ = ca.substitute(h_hat, x, x_bar) 92 | h_hat_ = ca.substitute(h_hat_, P, P_bar) 93 | 94 | f_x = ca.substitute(ca.jacobian(f,x), x, x_bar) 95 | 96 | h_hat_x = ca.substitute(ca.jacobian(h_hat,x), x, x_bar) 97 | h_hat_x = ca.substitute(h_hat_x, P, P_bar) 98 | h_hat_P = ca.substitute(ca.jacobian(h_hat, P), x, x_bar) 99 | h_hat_P = ca.substitute(h_hat_P, P, P_bar) 100 | h_hat_P_fun = ca.Function('h_hat_P_fun', [x,P], [ca.jacobian(h_hat,P)]) 101 | 102 | f_qp = ca.mtimes(f_x, dx) + 1/2*ca.mtimes(ca.mtimes(dx.T, M),dx) 103 | g_qp = ca.mtimes(h_hat_x, dx) 104 | 105 | # create QP solver 106 | pars = ca.vertcat(sigma, x_bar, P_bar) 107 | 108 | qp = {'x':ca.vertcat(dx), 'f':f_qp, 'g':g_qp, 'p':pars} 109 | qp_solver = ca.nlpsol("solver", "ipopt", qp) 110 | lbg = [-ca.inf] 111 | ubg = [0] 112 | 113 | # solve zoRO problem 114 | sol_x_zoro_v = [] 115 | x_bar = np.zeros((nx,1)) 116 | P_bar = np.ones((nw**2,1)) 117 | for i in range(len(sigma_v)): 118 | # SQP 119 | for j in range(maxiter): 120 | # eliminate P 121 | A_bar = A_fun(x_bar).full() 122 | b_bar = b_fun(x_bar).full() 123 | 124 | P_plus = np.linalg.solve(A_bar, (sigma_v[i]**2)*b_bar) 125 | 126 | # update rhs 127 | h_hat_bar = h_hat_fun(x_bar, P_bar) 128 | h_hat_P_bar = h_hat_P_fun(x_bar, P_bar).full() 129 | ubg[0] = (-h_hat_bar - ca.mtimes(h_hat_P_bar, P_plus - P_bar)).full() 130 | 131 | p = ca.vertcat(sigma_v[i], x_bar, P_bar).full() 132 | sol = qp_solver(x0=x_bar,lbg=lbg, ubg=ubg, p=p) 133 | status = nlp_solver.stats()['success'] 134 | 135 | if status != True: 136 | raise Exception('Solver failed at iteration {},{j}'.format(i,j)) 137 | 138 | dx = sol['x'] 139 | 140 | # step 141 | dP = (P_plus - P_bar) 142 | x_bar = x_bar + dx 143 | P_bar = P_bar + dP 144 | 145 | if np.linalg.norm(np.concatenate 146 | ([dx, dP])) < TOL: 147 | print('Solution found!') 148 | sol_x_zoro_v.append(ca.vertcat(x_bar,P_bar)) 149 | break 150 | 151 | if j >= maxiter-1: 152 | raise Exception('max. number of iterations reached at'\ 153 | ' iteration i={} j={}!'.format(i,j)) 154 | 155 | # plot 156 | fig, axs = plt.subplots(1, figsize=(4,3)) 157 | err = [] 158 | 159 | def plot_ellipse(P, c, axs, color, style,alpha): 160 | c.shape = (nx,1) 161 | npoints = 50 162 | theta = np.linspace(0, 2*np.pi) 163 | P_half = sqrtm(P) 164 | w = np.zeros((2,npoints)) 165 | for i in range(npoints): 166 | v = np.array([np.cos(theta[i]), np.sin(theta[i])]) 167 | v.shape = (2,1) 168 | w[:,i] = (c + np.matmul(P_half,v)).reshape(2,) 169 | axs.plot(w[0,:],w[1,:], color, alpha=alpha, linestyle=style, label='_no_legend_') 170 | 171 | def plot_constraint(axs, xlim, ylim): 172 | delta = 0.025 173 | x1 = np.arange(xlim[0], xlim[1], delta) 174 | x2 = np.arange(ylim[0], ylim[1], delta) 175 | X1, X2 = np.meshgrid(x1, x2) 176 | Z = h_fun(X1,X2) 177 | axs.contourf(X1,X2,Z,[0,10],color=['black'],alpha=0.3, label='_no_legend_') 178 | axs.contour(X1,X2,Z,[0],color=['black'], label='_no_legend_') 179 | 180 | def plot_objective(axs, xlim, ylim): 181 | delta = 0.025 182 | x1 = np.arange(xlim[0], xlim[1], delta) 183 | x2 = np.arange(ylim[0], ylim[1], delta) 184 | X1, X2 = np.meshgrid(x1, x2) 185 | Z = f_fun(X1,X2) 186 | axs.contour(X1,X2,Z,alpha=0.5, label='_no_legend_') 187 | 188 | sol_x_p = np.zeros((2, len(sigma_v))) 189 | sol_x_zoro_p = np.zeros((2, len(sigma_v))) 190 | c1 = 'black' 191 | c2 = 'black' 192 | s1 = 'dashed' 193 | s2 = 'solid' 194 | alpha1 = 0.5 195 | alpha2 = 1.0 196 | 197 | for i in range(len(sigma_v)): 198 | sol_x = sol_x_v[i].full()[0:2] 199 | sol_x.shape = (nx,) 200 | sol_x_p[:,i] = sol_x 201 | sol_x_zoro = sol_x_zoro_v[i].full()[0:2] 202 | sol_x_zoro.shape = (nx,) 203 | sol_x_zoro_p[:,i] = sol_x_zoro 204 | 205 | if i%3 == 0: 206 | P = sol_x_v[i].full()[nx:].reshape((nw, nw)) 207 | plot_ellipse(P, np.array([sol_x[0], sol_x[1]]), axs, c1, s1,alpha=alpha1) 208 | axs.plot(sol_x_p[0,i], sol_x_p[1,i],'black', marker='o', markersize=3,alpha=alpha1, label='_no_legend_') 209 | 210 | P = sol_x_zoro_v[i].full()[nx:].reshape((nw, nw)) 211 | plot_ellipse(P, np.array([sol_x_zoro[0], sol_x_zoro[1]]), axs, c2, s2,alpha=alpha2) 212 | axs.plot(sol_x_zoro_p[0,i], sol_x_zoro_p[1,i],c2, linestyle=s2, marker = 'o', markersize = 3,alpha=alpha2, label='_no_legend_') 213 | 214 | err.append(np.linalg.norm(sol_x - sol_x_zoro)) 215 | 216 | axs.plot(sol_x_p[0,:], sol_x_p[1,:], c1, linestyle=s1,alpha=alpha1) 217 | axs.plot(sol_x_zoro_p[0,:], sol_x_zoro_p[1,:], c2, linestyle=s2,alpha=alpha2) 218 | 219 | axs.grid() 220 | plot_constraint(axs, axs.get_xlim(), axs.get_ylim()) 221 | plot_objective(axs, axs.get_xlim(), axs.get_ylim()) 222 | axs.set_xlabel(r"$y_1$") 223 | axs.set_ylabel(r"$y_2$") 224 | axs.legend([r"exact", r"zoRO"],loc=2) 225 | 226 | plt.figure() 227 | plt.loglog(sigma_v, err) 228 | plt.xlabel(r"$\sigma$") 229 | plt.ylabel(r"$\|\tilde{z}(\sigma) - \bar{z}(\sigma) \|$") 230 | plt.grid() 231 | 232 | # fit poynomials 233 | p3 = np.polyfit(sigma_v, err,3) 234 | p3_line = sigma_v**3*p3[3] +sigma_v**2*p3[2] +sigma_v**1*p3[1] +sigma_v**0*p3[0] 235 | p2 = np.polyfit(sigma_v, err,2) 236 | p2_line = sigma_v**2*p2[2] +sigma_v**1*p2[1] +sigma_v**0*p2[0] 237 | p1 = np.polyfit(sigma_v, err,1) 238 | p1_line = sigma_v**1*p1[1] +sigma_v**0*p1[0] 239 | 240 | if UPDATE_FIG: 241 | fig.savefig('illustrative_example.pdf', dpi=300, bbox_inches="tight") 242 | 243 | plt.show() 244 | -------------------------------------------------------------------------------- /run_robust_control.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) The acados authors. 3 | # 4 | # This file is part of acados. 5 | # 6 | # The 2-Clause BSD License 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions are met: 10 | # 11 | # 1. Redistributions of source code must retain the above copyright notice, 12 | # this list of conditions and the following disclaimer. 13 | # 14 | # 2. Redistributions in binary form must reproduce the above copyright notice, 15 | # this list of conditions and the following disclaimer in the documentation 16 | # and/or other materials provided with the distribution. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE.; 29 | # 30 | 31 | import numpy as np 32 | import scipy.linalg 33 | import casadi as ca 34 | 35 | from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver 36 | 37 | from export_chain_mass_model import export_chain_mass_model 38 | from export_augmented_chain_mass_model import export_augmented_chain_mass_model 39 | from export_chain_mass_integrator import export_chain_mass_integrator 40 | 41 | from plot_utils import * 42 | from utils import * 43 | import matplotlib.pyplot as plt 44 | 45 | 46 | def run_robust_control(chain_params): 47 | # create ocp object to formulate the OCP 48 | ocp = AcadosOcp() 49 | 50 | # chain parameters 51 | n_mass = chain_params["n_mass"] 52 | M = chain_params["n_mass"] - 2 # number of intermediate masses 53 | Ts = chain_params["Ts"] 54 | Tsim = chain_params["Tsim"] 55 | N = chain_params["N"] 56 | u_init = chain_params["u_init"] 57 | with_wall = chain_params["with_wall"] 58 | yPosWall = chain_params["yPosWall"] 59 | m = chain_params["m"] 60 | D = chain_params["D"] 61 | L = chain_params["L"] 62 | perturb_scale = chain_params["perturb_scale"] 63 | 64 | nlp_iter = chain_params["nlp_iter"] 65 | nlp_tol = chain_params["nlp_tol"] 66 | save_results = chain_params["save_results"] 67 | show_plots = chain_params["show_plots"] 68 | seed = chain_params["seed"] 69 | 70 | np.random.seed(seed) 71 | 72 | nparam = 3*M 73 | W = perturb_scale * np.eye(nparam) 74 | 75 | # export model 76 | model = export_augmented_chain_mass_model(n_mass, m, D, L, Ts, W) 77 | 78 | # set model 79 | ocp.model = model 80 | 81 | nx_orig = M * 3 + (M+1)*3 82 | nx_aug = model.x.shape[0] 83 | 84 | nu = model.u.size()[0] 85 | ny = nx_orig + nu 86 | ny_e = nx_orig 87 | Tf = N * Ts 88 | 89 | # initial state 90 | xPosFirstMass = np.zeros((3,1)) 91 | xEndRef = np.zeros((3,1)) 92 | xEndRef[0] = L * (M+1) * 6 93 | pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass) 94 | 95 | xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef) 96 | 97 | x0 = xrest 98 | 99 | P0_mat = 1e-3 * np.eye(nx_orig) 100 | P0_vec = sym_mat2vec(P0_mat) 101 | 102 | # set dimensions 103 | ocp.solver_options.N_horizon = N 104 | 105 | # set cost module 106 | ocp.cost.cost_type = 'LINEAR_LS' 107 | ocp.cost.cost_type_e = 'LINEAR_LS' 108 | 109 | Q = 2*np.diagflat( np.ones((nx_orig, 1)) ) 110 | q_diag = np.ones((nx_orig,1)) 111 | strong_penalty = M+1 112 | q_diag[3*M] = strong_penalty 113 | q_diag[3*M+1] = strong_penalty 114 | q_diag[3*M+2] = strong_penalty 115 | Q = 2*np.diagflat( q_diag ) 116 | 117 | R = 2*np.diagflat( 1e-2 * np.ones((nu, 1)) ) 118 | 119 | ocp.cost.W = scipy.linalg.block_diag(Q, R) 120 | ocp.cost.W_e = Q 121 | 122 | ocp.cost.Vx = np.zeros((ny, nx_aug)) 123 | ocp.cost.Vx[:nx_orig,:nx_orig] = np.eye(nx_orig) 124 | 125 | Vu = np.zeros((ny, nu)) 126 | Vu[nx_orig:nx_orig+nu, :] = np.eye(nu) 127 | ocp.cost.Vu = Vu 128 | 129 | Vx_e = np.zeros((ny_e, nx_aug)) 130 | Vx_e[:nx_orig, :nx_orig] = np.eye(nx_orig) 131 | ocp.cost.Vx_e = Vx_e 132 | 133 | 134 | yref = np.vstack((xrest, np.zeros((nu,1)))).flatten() 135 | ocp.cost.yref = yref 136 | ocp.cost.yref_e = xrest.flatten() 137 | 138 | # set constraints 139 | umax = 1*np.ones((nu,)) 140 | 141 | ocp.constraints.constr_type = 'BGH' 142 | ocp.constraints.lbu = -umax 143 | ocp.constraints.ubu = umax 144 | x0_aug = np.hstack((x0.flatten(), P0_vec)) 145 | ocp.constraints.x0 = x0_aug.reshape((nx_aug,)) 146 | ocp.constraints.idxbu = np.array(range(nu)) 147 | 148 | # disturbances 149 | 150 | # wall constraint 151 | if with_wall: 152 | # slacks 153 | L2_pen = 1e3 154 | L1_pen = 1 155 | 156 | # # nominal 157 | # nbx = M + 1 158 | # Jbx = np.zeros((nbx, nx_orig)) 159 | # for i in range(nbx): 160 | # Jbx[i, 3*i+1] = 1.0 161 | 162 | # ocp.constraints.Jbx = Jbx 163 | # ocp.constraints.lbx = yPosWall * np.ones((nbx,)) 164 | # ocp.constraints.ubx = 1e9 * np.ones((nbx,)) 165 | # ocp.constraints.Jsbx = np.eye(nbx) 166 | 167 | # ocp.cost.Zl = L2_pen * np.ones((nbx,)) 168 | # ocp.cost.Zu = L2_pen * np.ones((nbx,)) 169 | # ocp.cost.zl = L1_pen * np.ones((nbx,)) 170 | # ocp.cost.zu = L1_pen * np.ones((nbx,)) 171 | 172 | nh = M + 1 173 | 174 | ocp.constraints.lh = yPosWall * np.ones((nh,)) 175 | ocp.constraints.uh = 1e9 * np.ones((nh,)) 176 | ocp.constraints.Jsh = np.eye(nh) 177 | h_expr = ca.SX.zeros(nh,1) 178 | for j in range(nh): 179 | P_mat = vec2sym_mat( model.x[nx_orig:], nx_orig ) 180 | # Note: lower bound, therefore need to substract the backoff term 181 | h_expr[j] = model.x[3*j+1] - ca.sqrt(P_mat[3*j+1,3*j+1]) 182 | ocp.model.con_h_expr = h_expr 183 | 184 | ocp.cost.Zl = L2_pen * np.ones((nh,)) 185 | ocp.cost.Zu = L2_pen * np.ones((nh,)) 186 | ocp.cost.zl = L1_pen * np.ones((nh,)) 187 | ocp.cost.zu = L1_pen * np.ones((nh,)) 188 | 189 | 190 | 191 | # solver options 192 | ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES 193 | # ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' # FULL_CONDENSING_QPOASES 194 | 195 | ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' 196 | ocp.solver_options.integrator_type = 'IRK' 197 | ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI 198 | ocp.solver_options.nlp_solver_max_iter = nlp_iter 199 | 200 | ocp.solver_options.sim_method_num_stages = 2 201 | ocp.solver_options.sim_method_num_steps = 2 202 | ocp.solver_options.qp_solver_cond_N = N # N TODO 203 | ocp.solver_options.tol = nlp_tol 204 | ocp.solver_options.qp_tol = nlp_tol 205 | ocp.solver_options.print_level = 0 206 | 207 | # set prediction horizon 208 | ocp.solver_options.tf = Tf 209 | acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 210 | print("Generated AcadosOcpSolver successfully") 211 | # acados_integrator = AcadosSimSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 212 | acados_integrator = export_chain_mass_integrator(n_mass, m, D, L) 213 | 214 | #%% get initial state from xrest 215 | xcurrent = x0.reshape((nx_orig,)) 216 | for i in range(5): 217 | acados_integrator.set("x", xcurrent) 218 | acados_integrator.set("u", u_init) 219 | 220 | status = acados_integrator.solve() 221 | if status != 0: 222 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 223 | 224 | # update state 225 | xcurrent = acados_integrator.get("x") 226 | 227 | #%% actual simulation 228 | N_sim = int(np.floor(Tsim/Ts)) 229 | simX = np.ndarray((N_sim+1, nx_orig)) 230 | simU = np.ndarray((N_sim, nu)) 231 | wall_dist = np.zeros((N_sim,)) 232 | simX[0,:] = xcurrent 233 | 234 | timings = np.zeros((N_sim,)) 235 | Pposdef = np.zeros((N_sim, N)) 236 | 237 | xcurrent_aug = np.hstack((xcurrent.flatten(), P0_vec)) 238 | 239 | # closed loop 240 | for i in range(N_sim): 241 | # solve ocp 242 | acados_ocp_solver.set(0, "lbx", xcurrent_aug) 243 | acados_ocp_solver.set(0, "ubx", xcurrent_aug) 244 | 245 | status = acados_ocp_solver.solve() 246 | acados_ocp_solver.print_statistics() 247 | 248 | if status != 0: 249 | raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i)) 250 | 251 | simU[i,:] = acados_ocp_solver.get(0, "u") 252 | 253 | # simulate system 254 | acados_integrator.set("x", xcurrent) 255 | acados_integrator.set("u", simU[i,:]) 256 | 257 | pertubation = sampleFromEllipsoid(np.zeros((nparam,)), W) 258 | acados_integrator.set("p", pertubation) 259 | 260 | status = acados_integrator.solve() 261 | if status != 0: 262 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 263 | 264 | # update state 265 | xcurrent = acados_integrator.get("x") 266 | simX[i+1,:] = xcurrent 267 | xcurrent_aug = np.hstack((xcurrent.flatten(), P0_vec)) 268 | 269 | # get P covariances 270 | for j in range(N): 271 | xocp = acados_ocp_solver.get(j, "x") 272 | P = vec2sym_mat(xocp[nx_orig:], nx_orig) 273 | Pposdef[i,j] = is_pos_def(P) 274 | 275 | timings[i] = acados_ocp_solver.get_stats("time_tot")[0] 276 | 277 | yPos = xcurrent[range(1,3*M+1,3)] 278 | wall_dist[i] = np.min(yPos - yPosWall) 279 | print("time i = ", str(i), " dist2wall ", str(wall_dist[i])) 280 | 281 | print("dist2wall (minimum over simulation) ", str(np.min(wall_dist))) 282 | print("average time OCP: ", str(np.average(timings))) 283 | 284 | #%% plot results 285 | if show_plots: 286 | plot_chain_control_traj(simU) 287 | plot_chain_position_traj(simX, yPosWall=yPosWall) 288 | plot_chain_velocity_traj(simX) 289 | # plot_chain_position(simX[-1,:], xPosFirstMass) 290 | animate_chain_position(simX, xPosFirstMass, yPosWall=yPosWall) 291 | 292 | plt.show() 293 | 294 | #%% save results 295 | if save_results: 296 | ID = "robust" 297 | timings_Pprop = np.zeros((N_sim,)) 298 | save_closed_loop_results_as_json(ID, timings, timings_Pprop, wall_dist, chain_params) 299 | -------------------------------------------------------------------------------- /run_fastzoro_robust_control.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) The acados authors. 3 | # 4 | # This file is part of acados. 5 | # 6 | # The 2-Clause BSD License 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions are met: 10 | # 11 | # 1. Redistributions of source code must retain the above copyright notice, 12 | # this list of conditions and the following disclaimer. 13 | # 14 | # 2. Redistributions in binary form must reproduce the above copyright notice, 15 | # this list of conditions and the following disclaimer in the documentation 16 | # and/or other materials provided with the distribution. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE.; 29 | # 30 | 31 | import sys 32 | import os 33 | from casadi.casadi import sqrt 34 | import numpy as np 35 | import scipy.linalg 36 | 37 | from time import process_time 38 | 39 | from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver, ZoroDescription 40 | 41 | from export_chain_mass_model import export_chain_mass_model 42 | from export_disturbed_chain_mass_model import export_disturbed_chain_mass_model 43 | from export_chain_mass_integrator import export_chain_mass_integrator 44 | 45 | from plot_utils import * 46 | from utils import * 47 | import matplotlib.pyplot as plt 48 | 49 | def run_fastzoro_robust_control(chain_params): 50 | ID = "fastzoRO" 51 | 52 | # create ocp object to formulate the OCP 53 | ocp = AcadosOcp() 54 | 55 | # chain parameters 56 | n_mass = chain_params["n_mass"] 57 | M = chain_params["n_mass"] - 2 # number of intermediate masses 58 | Ts = chain_params["Ts"] 59 | Tsim = chain_params["Tsim"] 60 | N = chain_params["N"] 61 | u_init = chain_params["u_init"] 62 | with_wall = chain_params["with_wall"] 63 | yPosWall = chain_params["yPosWall"] 64 | m = chain_params["m"] 65 | D = chain_params["D"] 66 | L = chain_params["L"] 67 | perturb_scale = chain_params["perturb_scale"] 68 | 69 | nlp_iter = chain_params["nlp_iter"] 70 | nlp_tol = chain_params["nlp_tol"] 71 | save_results = chain_params["save_results"] 72 | show_plots = chain_params["show_plots"] 73 | seed = chain_params["seed"] 74 | 75 | 76 | np.random.seed(seed) 77 | 78 | nparam = 3*M 79 | W = perturb_scale * np.eye(nparam) 80 | 81 | # export model 82 | model = export_disturbed_chain_mass_model(n_mass, m, D, L) 83 | model.name = model.name + "_robust" 84 | 85 | # set model 86 | ocp.model = model 87 | 88 | nx = model.x.size()[0] 89 | nu = model.u.size()[0] 90 | ny = nx + nu 91 | ny_e = nx 92 | Tf = N * Ts 93 | 94 | # initial state 95 | xPosFirstMass = np.zeros((3,1)) 96 | xEndRef = np.zeros((3,1)) 97 | xEndRef[0] = L * (M+1) * 6 98 | pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass) 99 | 100 | xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef) 101 | 102 | x0 = xrest 103 | 104 | # set dimensions 105 | ocp.solver_options.N_horizon = N 106 | 107 | # set cost module 108 | ocp.cost.cost_type = 'LINEAR_LS' 109 | ocp.cost.cost_type_e = 'LINEAR_LS' 110 | 111 | Q = 2*np.diagflat( np.ones((nx, 1)) ) 112 | q_diag = np.ones((nx,1)) 113 | strong_penalty = M+1 114 | q_diag[3*M] = strong_penalty 115 | q_diag[3*M+1] = strong_penalty 116 | q_diag[3*M+2] = strong_penalty 117 | Q = 2*np.diagflat( q_diag ) 118 | 119 | R = 2*np.diagflat( 1e-2 * np.ones((nu, 1)) ) 120 | 121 | ocp.cost.W = scipy.linalg.block_diag(Q, R) 122 | ocp.cost.W_e = Q 123 | 124 | ocp.cost.Vx = np.zeros((ny, nx)) 125 | ocp.cost.Vx[:nx,:nx] = np.eye(nx) 126 | 127 | Vu = np.zeros((ny, nu)) 128 | Vu[nx:nx+nu, :] = np.eye(nu) 129 | ocp.cost.Vu = Vu 130 | 131 | ocp.cost.Vx_e = np.eye(nx) 132 | 133 | # import pdb; pdb.set_trace() 134 | yref = np.vstack((xrest, np.zeros((nu,1)))).flatten() 135 | ocp.cost.yref = yref 136 | ocp.cost.yref_e = xrest.flatten() 137 | 138 | # set constraints 139 | umax = 1*np.ones((nu,)) 140 | 141 | ocp.constraints.constr_type = 'BGH' 142 | ocp.constraints.lbu = -umax 143 | ocp.constraints.ubu = umax 144 | ocp.constraints.x0 = x0.reshape((nx,)) 145 | ocp.constraints.idxbu = np.array(range(nu)) 146 | 147 | # disturbances 148 | nparam = 3*M 149 | ocp.parameter_values = np.zeros((nparam,)) 150 | 151 | # wall constraint 152 | if with_wall: 153 | nbx = M + 1 154 | Jbx = np.zeros((nbx,nx)) 155 | for i in range(nbx): 156 | Jbx[i, 3*i+1] = 1.0 157 | 158 | ocp.constraints.Jbx = Jbx 159 | ocp.constraints.lbx = yPosWall * np.ones((nbx,)) 160 | ocp.constraints.ubx = 1e9 * np.ones((nbx,)) 161 | 162 | # slacks 163 | ocp.constraints.Jsbx = np.eye(nbx) 164 | L2_pen = 1e3 165 | L1_pen = 1 166 | ocp.cost.Zl = L2_pen * np.ones((nbx,)) 167 | ocp.cost.Zu = L2_pen * np.ones((nbx,)) 168 | ocp.cost.zl = L1_pen * np.ones((nbx,)) 169 | ocp.cost.zu = L1_pen * np.ones((nbx,)) 170 | 171 | 172 | # solver options 173 | ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES 174 | ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' 175 | ocp.solver_options.integrator_type = 'IRK' 176 | ocp.solver_options.nlp_solver_type = 'SQP_RTI' # SQP, SQP_RTI 177 | 178 | ocp.solver_options.sim_method_num_stages = 2 179 | ocp.solver_options.sim_method_num_steps = 2 180 | ocp.solver_options.qp_solver_cond_N = N 181 | ocp.solver_options.tol = nlp_tol 182 | ocp.solver_options.qp_tol = nlp_tol 183 | ocp.solver_options.nlp_solver_max_iter = nlp_iter 184 | # ocp.solver_options.qp_solver_iter_max = 500 185 | # ocp.solver_options.qp_solver_max_iter = 100 186 | 187 | # set prediction horizon 188 | ocp.solver_options.tf = Tf 189 | 190 | ocp.code_export_directory = "c_generated_code" + "_" + ID 191 | 192 | # custom update: disturbance propagation 193 | ocp.solver_options.custom_update_filename = 'custom_update_function.c' 194 | ocp.solver_options.custom_update_header_filename = 'custom_update_function.h' 195 | ocp.solver_options.custom_update_copy = False 196 | ocp.solver_options.custom_templates = [ 197 | ('custom_update_function_zoro_template.in.c', 'custom_update_function.c'), 198 | ('custom_update_function_zoro_template.in.h', 'custom_update_function.h'), 199 | ] 200 | # zoro stuff 201 | zoro_description = ZoroDescription() 202 | # uncertainty propagation: P_{k+1} = (A_k+B_kK) @ P_k @ (A_k+B_kK)^T + G @ W @ G^T 203 | # G.shape = (nx, nw), W.shape = (nw, nw) 204 | zoro_description.fdbk_K_mat = np.zeros((nu, nx)) 205 | zoro_description.unc_jac_G_mat = np.vstack(( np.zeros((nx - nparam, nparam)), np.eye(nparam))) 206 | zoro_description.P0_mat = 1e-3 * np.eye(nx) 207 | zoro_description.W_mat = W*Ts 208 | zoro_description.idx_lbx_t = list(range(nbx)) 209 | ocp.zoro_description = zoro_description 210 | 211 | # acados_integrator = AcadosSimSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 212 | acados_integrator = export_chain_mass_integrator(n_mass, m, D, L) 213 | 214 | ## OCP solver 215 | # ctypes 216 | acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 217 | 218 | # cython 219 | # AcadosOcpSolver.generate(ocp, json_file='acados_ocp_' + model.name + '.json') 220 | # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) 221 | # acados_ocp_solver = AcadosOcpSolver.create_cython_solver('acados_ocp_' + model.name + '.json') 222 | 223 | 224 | #%% get initial state from xrest 225 | xcurrent = x0.reshape((nx,)) 226 | for i in range(5): 227 | acados_integrator.set("x", xcurrent) 228 | acados_integrator.set("u", u_init) 229 | 230 | status = acados_integrator.solve() 231 | if status != 0: 232 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 233 | 234 | # update state 235 | xcurrent = acados_integrator.get("x") 236 | 237 | #%% actual simulation 238 | N_sim = int(np.floor(Tsim/Ts)) 239 | simX = np.ndarray((N_sim+1, nx)) 240 | simU = np.ndarray((N_sim, nu)) 241 | wall_dist = np.zeros((N_sim,)) 242 | 243 | timings = np.zeros((N_sim,)) 244 | timings_Pprop = np.zeros((N_sim,)) 245 | 246 | simX[0,:] = xcurrent 247 | 248 | nbx = M + 1 249 | lbx = np.zeros((nbx,)) 250 | 251 | # closed loop 252 | for i in range(N_sim): 253 | 254 | # solve ocp 255 | acados_ocp_solver.set(0, "lbx", xcurrent) 256 | acados_ocp_solver.set(0, "ubx", xcurrent) 257 | 258 | if ocp.solver_options.nlp_solver_type == 'SQP_RTI': 259 | # SQP loop: 260 | for i_sqp in range(ocp.solver_options.nlp_solver_max_iter): 261 | 262 | # preparation rti_phase 263 | acados_ocp_solver.options_set('rti_phase', 1) 264 | status = acados_ocp_solver.solve() 265 | timings[i] += acados_ocp_solver.get_stats("time_tot")[0] 266 | 267 | # Disturbance propagation in custom_update 268 | t1 = process_time() 269 | acados_ocp_solver.custom_update([]) 270 | timings_Pprop[i] += process_time() - t1 271 | 272 | # feedback rti_phase 273 | acados_ocp_solver.options_set('rti_phase', 2) 274 | status = acados_ocp_solver.solve() 275 | timings[i] += acados_ocp_solver.get_stats("time_tot")[0] 276 | 277 | # check on residuals and terminate loop. 278 | # acados_ocp_solver.print_statistics() # encapsulates: stat = acados_ocp_solver.get_stats("statistics") 279 | residuals = acados_ocp_solver.get_residuals() 280 | # print("residuals after ", i_sqp, "SQP_RTI iterations:\n", residuals) 281 | 282 | if status != 0: 283 | raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i)) 284 | 285 | if max(residuals) < nlp_tol: 286 | break 287 | 288 | else: 289 | status = acados_ocp_solver.solve() 290 | # acados_ocp_solver.print_statistics() # encapsulates: stat = acados_ocp_solver.get_stats("statistics") 291 | if status != 0: 292 | raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i)) 293 | 294 | simU[i,:] = acados_ocp_solver.get(0, "u") 295 | 296 | # only LQR 297 | # simU[i,:] = K @ (xcurrent - xrest.flatten()) 298 | 299 | print("control at time", i, ":", simU[i,:]) 300 | 301 | # simulate system 302 | acados_integrator.set("x", xcurrent) 303 | acados_integrator.set("u", simU[i,:]) 304 | 305 | pertubation = sampleFromEllipsoid(np.zeros((nparam,)), W) 306 | # acados_integrator.set("p", pertubation) 307 | 308 | status = acados_integrator.solve() 309 | if status != 0: 310 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 311 | 312 | # update state 313 | # import pdb; pdb.set_trace() 314 | xcurrent = acados_integrator.get("x") + Ts * np.hstack((np.zeros(((M+1)*3,)), pertubation)) 315 | simX[i+1,:] = xcurrent 316 | 317 | # xOcpPredict = acados_ocp_solver.get(1, "x") 318 | # print("model mismatch = ", str(np.max(xOcpPredict - xcurrent))) 319 | yPos = xcurrent[range(1,3*M+2,3)] 320 | wall_dist[i] = np.min(yPos - yPosWall) 321 | print("time i = ", str(i), " dist2wall ", str(wall_dist[i])) 322 | 323 | print("dist2wall (minimum over simulation) ", str(np.min(wall_dist))) 324 | 325 | #%% plot results 326 | if show_plots: 327 | plot_chain_control_traj(simU) 328 | plot_chain_position_traj(simX, yPosWall=yPosWall) 329 | plot_chain_velocity_traj(simX) 330 | # plot_chain_position(simX[-1,:], xPosFirstMass) 331 | animate_chain_position(simX, xPosFirstMass, yPosWall=yPosWall) 332 | 333 | plt.show() 334 | 335 | 336 | #%% save results 337 | if save_results: 338 | save_closed_loop_results_as_json(ID, timings, timings_Pprop, wall_dist, chain_params) 339 | -------------------------------------------------------------------------------- /run_tailored_robust_control.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) The acados authors. 3 | # 4 | # This file is part of acados. 5 | # 6 | # The 2-Clause BSD License 7 | # 8 | # Redistribution and use in source and binary forms, with or without 9 | # modification, are permitted provided that the following conditions are met: 10 | # 11 | # 1. Redistributions of source code must retain the above copyright notice, 12 | # this list of conditions and the following disclaimer. 13 | # 14 | # 2. Redistributions in binary form must reproduce the above copyright notice, 15 | # this list of conditions and the following disclaimer in the documentation 16 | # and/or other materials provided with the distribution. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE.; 29 | # 30 | 31 | import sys 32 | import os 33 | from casadi.casadi import sqrt 34 | import numpy as np 35 | import scipy.linalg 36 | 37 | from time import process_time 38 | 39 | from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver 40 | 41 | from export_chain_mass_model import export_chain_mass_model 42 | from export_disturbed_chain_mass_model import export_disturbed_chain_mass_model 43 | from export_chain_mass_integrator import export_chain_mass_integrator 44 | 45 | from plot_utils import * 46 | from utils import * 47 | import matplotlib.pyplot as plt 48 | 49 | def run_tailored_robust_control(chain_params): 50 | ID = "zoRO" 51 | 52 | # create ocp object to formulate the OCP 53 | ocp = AcadosOcp() 54 | 55 | # chain parameters 56 | n_mass = chain_params["n_mass"] 57 | M = chain_params["n_mass"] - 2 # number of intermediate masses 58 | Ts = chain_params["Ts"] 59 | Tsim = chain_params["Tsim"] 60 | N = chain_params["N"] 61 | u_init = chain_params["u_init"] 62 | with_wall = chain_params["with_wall"] 63 | yPosWall = chain_params["yPosWall"] 64 | m = chain_params["m"] 65 | D = chain_params["D"] 66 | L = chain_params["L"] 67 | perturb_scale = chain_params["perturb_scale"] 68 | 69 | nlp_iter = chain_params["nlp_iter"] 70 | nlp_tol = chain_params["nlp_tol"] 71 | save_results = chain_params["save_results"] 72 | show_plots = chain_params["show_plots"] 73 | seed = chain_params["seed"] 74 | 75 | 76 | np.random.seed(seed) 77 | 78 | nparam = 3*M 79 | W = perturb_scale * np.eye(nparam) 80 | 81 | # export model 82 | model = export_disturbed_chain_mass_model(n_mass, m, D, L) 83 | model.name = model.name + "_robust" 84 | 85 | # set model 86 | ocp.model = model 87 | 88 | nx = model.x.size()[0] 89 | nu = model.u.size()[0] 90 | ny = nx + nu 91 | ny_e = nx 92 | Tf = N * Ts 93 | 94 | # initial state 95 | xPosFirstMass = np.zeros((3,1)) 96 | xEndRef = np.zeros((3,1)) 97 | xEndRef[0] = L * (M+1) * 6 98 | pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass) 99 | 100 | xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef) 101 | 102 | x0 = xrest 103 | 104 | # set dimensions 105 | ocp.solver_options.N_horizon = N 106 | 107 | # set cost module 108 | ocp.cost.cost_type = 'LINEAR_LS' 109 | ocp.cost.cost_type_e = 'LINEAR_LS' 110 | 111 | Q = 2*np.diagflat( np.ones((nx, 1)) ) 112 | q_diag = np.ones((nx,1)) 113 | strong_penalty = M+1 114 | q_diag[3*M] = strong_penalty 115 | q_diag[3*M+1] = strong_penalty 116 | q_diag[3*M+2] = strong_penalty 117 | Q = 2*np.diagflat( q_diag ) 118 | 119 | R = 2*np.diagflat( 1e-2 * np.ones((nu, 1)) ) 120 | 121 | ocp.cost.W = scipy.linalg.block_diag(Q, R) 122 | ocp.cost.W_e = Q 123 | 124 | ocp.cost.Vx = np.zeros((ny, nx)) 125 | ocp.cost.Vx[:nx,:nx] = np.eye(nx) 126 | 127 | Vu = np.zeros((ny, nu)) 128 | Vu[nx:nx+nu, :] = np.eye(nu) 129 | ocp.cost.Vu = Vu 130 | 131 | ocp.cost.Vx_e = np.eye(nx) 132 | 133 | # import pdb; pdb.set_trace() 134 | yref = np.vstack((xrest, np.zeros((nu,1)))).flatten() 135 | ocp.cost.yref = yref 136 | ocp.cost.yref_e = xrest.flatten() 137 | 138 | # set constraints 139 | umax = 1*np.ones((nu,)) 140 | 141 | ocp.constraints.constr_type = 'BGH' 142 | ocp.constraints.lbu = -umax 143 | ocp.constraints.ubu = umax 144 | ocp.constraints.x0 = x0.reshape((nx,)) 145 | ocp.constraints.idxbu = np.array(range(nu)) 146 | 147 | # disturbances 148 | nparam = 3*M 149 | ocp.parameter_values = np.zeros((nparam,)) 150 | 151 | # wall constraint 152 | if with_wall: 153 | nbx = M + 1 154 | Jbx = np.zeros((nbx,nx)) 155 | for i in range(nbx): 156 | Jbx[i, 3*i+1] = 1.0 157 | 158 | ocp.constraints.Jbx = Jbx 159 | ocp.constraints.lbx = yPosWall * np.ones((nbx,)) 160 | ocp.constraints.ubx = 1e9 * np.ones((nbx,)) 161 | 162 | # slacks 163 | ocp.constraints.Jsbx = np.eye(nbx) 164 | L2_pen = 1e3 165 | L1_pen = 1 166 | ocp.cost.Zl = L2_pen * np.ones((nbx,)) 167 | ocp.cost.Zu = L2_pen * np.ones((nbx,)) 168 | ocp.cost.zl = L1_pen * np.ones((nbx,)) 169 | ocp.cost.zu = L1_pen * np.ones((nbx,)) 170 | 171 | 172 | # solver options 173 | ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES 174 | ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' 175 | ocp.solver_options.integrator_type = 'IRK' 176 | ocp.solver_options.nlp_solver_type = 'SQP_RTI' # SQP, SQP_RTI 177 | 178 | ocp.solver_options.sim_method_num_stages = 2 179 | ocp.solver_options.sim_method_num_steps = 2 180 | ocp.solver_options.qp_solver_cond_N = N 181 | ocp.solver_options.tol = nlp_tol 182 | ocp.solver_options.qp_tol = nlp_tol 183 | ocp.solver_options.nlp_solver_max_iter = nlp_iter 184 | # ocp.solver_options.qp_solver_iter_max = 500 185 | # ocp.solver_options.qp_solver_max_iter = 100 186 | 187 | # set prediction horizon 188 | ocp.solver_options.tf = Tf 189 | 190 | ocp.code_export_directory = "c_generated_code" + "_" + ID 191 | 192 | # acados_integrator = AcadosSimSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 193 | acados_integrator = export_chain_mass_integrator(n_mass, m, D, L) 194 | acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 195 | # AcadosOcpSolver.generate(ocp, json_file='acados_ocp_' + model.name + '.json') 196 | # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) 197 | # acados_ocp_solver = AcadosOcpSolver.create_cython_solver('acados_ocp_' + model.name + '.json') 198 | 199 | 200 | #%% get initial state from xrest 201 | xcurrent = x0.reshape((nx,)) 202 | for i in range(5): 203 | acados_integrator.set("x", xcurrent) 204 | acados_integrator.set("u", u_init) 205 | 206 | status = acados_integrator.solve() 207 | if status != 0: 208 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 209 | 210 | # update state 211 | xcurrent = acados_integrator.get("x") 212 | 213 | #%% actual simulation 214 | N_sim = int(np.floor(Tsim/Ts)) 215 | simX = np.ndarray((N_sim+1, nx)) 216 | simU = np.ndarray((N_sim, nu)) 217 | wall_dist = np.zeros((N_sim,)) 218 | 219 | timings = np.zeros((N_sim,)) 220 | timings_Pprop = np.zeros((N_sim,)) 221 | 222 | simX[0,:] = xcurrent 223 | 224 | P0_mat = 1e-3 * np.eye(nx) 225 | P_mat_list = [None] * (N+1) 226 | P_mat_list[0] = P0_mat 227 | 228 | nbx = M + 1 229 | lbx = np.zeros((nbx,)) 230 | 231 | # closed loop 232 | for i in range(N_sim): 233 | 234 | # solve ocp 235 | acados_ocp_solver.set(0, "lbx", xcurrent) 236 | acados_ocp_solver.set(0, "ubx", xcurrent) 237 | 238 | if ocp.solver_options.nlp_solver_type == 'SQP_RTI': 239 | # SQP loop: 240 | for i_sqp in range(ocp.solver_options.nlp_solver_max_iter): 241 | 242 | # preparation rti_phase 243 | acados_ocp_solver.options_set('rti_phase', 1) 244 | status = acados_ocp_solver.solve() 245 | timings[i] += acados_ocp_solver.get_stats("time_tot")[0] 246 | 247 | # hardcode B for discrete time disturbance 248 | B = np.vstack(( np.zeros((nx - nparam, nparam)), np.eye(nparam))) 249 | for stage in range(N): 250 | # get A matrices 251 | A = acados_ocp_solver.get_from_qp_in(stage, "A") 252 | # propagate Ps 253 | 254 | P_mat_old = P_mat_list[stage+1] 255 | 256 | t1 = process_time() 257 | P_mat_list[stage+1] = P_propagation(P_mat_list[stage], A, B, W*Ts) 258 | timings_Pprop[i] += process_time() - t1 259 | 260 | if isinstance(P_mat_old, type(None)): 261 | # i == 0 262 | dP_bar = np.zeros((nx,nx)) 263 | else: 264 | dP_bar = P_mat_list[stage+1] - P_mat_old 265 | 266 | # compute backoff using P 267 | for j in range(nbx): 268 | P = P_mat_list[stage] 269 | ibx = 3*j+1 270 | lbx[j] = yPosWall + np.sqrt(P[ibx, ibx]) + .5 * dP_bar[ibx, ibx] / np.sqrt(P[ibx, ibx]) 271 | 272 | # set bounds with backoff (nabla h available since h linear) 273 | if stage > 0: 274 | acados_ocp_solver.constraints_set(stage, "lbx", lbx) 275 | 276 | # - h <-> wall constraint 277 | 278 | # feedback rti_phase 279 | acados_ocp_solver.options_set('rti_phase', 2) 280 | status = acados_ocp_solver.solve() 281 | timings[i] += acados_ocp_solver.get_stats("time_tot")[0] 282 | 283 | # check on residuals and terminate loop. 284 | # acados_ocp_solver.print_statistics() # encapsulates: stat = acados_ocp_solver.get_stats("statistics") 285 | residuals = acados_ocp_solver.get_residuals() 286 | # print("residuals after ", i_sqp, "SQP_RTI iterations:\n", residuals) 287 | 288 | if status != 0: 289 | raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i)) 290 | 291 | if max(residuals) < nlp_tol: 292 | break 293 | 294 | else: 295 | status = acados_ocp_solver.solve() 296 | # acados_ocp_solver.print_statistics() # encapsulates: stat = acados_ocp_solver.get_stats("statistics") 297 | if status != 0: 298 | raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i)) 299 | 300 | simU[i,:] = acados_ocp_solver.get(0, "u") 301 | 302 | # only LQR 303 | # simU[i,:] = K @ (xcurrent - xrest.flatten()) 304 | 305 | print("control at time", i, ":", simU[i,:]) 306 | 307 | # simulate system 308 | acados_integrator.set("x", xcurrent) 309 | acados_integrator.set("u", simU[i,:]) 310 | 311 | pertubation = sampleFromEllipsoid(np.zeros((nparam,)), W) 312 | # acados_integrator.set("p", pertubation) 313 | 314 | status = acados_integrator.solve() 315 | if status != 0: 316 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 317 | 318 | # update state 319 | # import pdb; pdb.set_trace() 320 | xcurrent = acados_integrator.get("x") + Ts * np.hstack((np.zeros(((M+1)*3,)), pertubation)) 321 | simX[i+1,:] = xcurrent 322 | 323 | # xOcpPredict = acados_ocp_solver.get(1, "x") 324 | # print("model mismatch = ", str(np.max(xOcpPredict - xcurrent))) 325 | yPos = xcurrent[range(1,3*M+2,3)] 326 | wall_dist[i] = np.min(yPos - yPosWall) 327 | print("time i = ", str(i), " dist2wall ", str(wall_dist[i])) 328 | 329 | print("dist2wall (minimum over simulation) ", str(np.min(wall_dist))) 330 | 331 | #%% plot results 332 | if show_plots: 333 | plot_chain_control_traj(simU) 334 | plot_chain_position_traj(simX, yPosWall=yPosWall) 335 | plot_chain_velocity_traj(simX) 336 | # plot_chain_position(simX[-1,:], xPosFirstMass) 337 | animate_chain_position(simX, xPosFirstMass, yPosWall=yPosWall) 338 | 339 | plt.show() 340 | 341 | 342 | #%% save results 343 | if save_results: 344 | save_closed_loop_results_as_json(ID, timings, timings_Pprop, wall_dist, chain_params) 345 | -------------------------------------------------------------------------------- /plot_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | from mpl_toolkits.mplot3d import Axes3D 5 | import matplotlib.animation as animation 6 | 7 | 8 | def get_latex_plot_params(): 9 | params = {'backend': 'ps', 10 | 'text.latex.preamble': r"\usepackage{gensymb} \usepackage{amsmath}", 11 | 'axes.labelsize': 12, 12 | 'axes.titlesize': 12, 13 | 'legend.fontsize': 12, 14 | 'xtick.labelsize': 12, 15 | 'ytick.labelsize': 12, 16 | 'text.usetex': True, 17 | 'font.family': 'serif' 18 | } 19 | 20 | return params 21 | 22 | 23 | def plot_chain_position_traj(simX, yPosWall=None): 24 | plt.figure() 25 | nx = simX.shape[1] 26 | N = simX.shape[0] 27 | M = int((nx/3 -1)/2) 28 | # plt.title('Chain position trajectory') 29 | 30 | for i in range(M+1): 31 | plt.subplot(M+1, 3, 3*i+1) 32 | plt.ylabel('x') 33 | plt.plot(simX[:, 3*i]) 34 | plt.grid(True) 35 | 36 | plt.subplot(M+1, 3, 3*i+2) 37 | plt.ylabel('y') 38 | plt.plot(simX[:, 3*i+1]) 39 | if not yPosWall == None: 40 | plt.plot(yPosWall*np.ones((N,))) 41 | plt.grid(True) 42 | 43 | plt.subplot(M+1, 3, 3*i+3) 44 | plt.ylabel('z') 45 | plt.plot(simX[:, 3*i+2]) 46 | plt.grid(True) 47 | 48 | 49 | 50 | def plot_chain_velocity_traj(simX): 51 | plt.figure() 52 | nx = simX.shape[1] 53 | M = int((nx/3 -1)/2) 54 | 55 | simX = simX[:, (M+1)*3:] 56 | 57 | for i in range(M): 58 | plt.subplot(M, 3, 3*i+1) 59 | plt.plot(simX[:, 3*i]) 60 | plt.ylabel('vx') 61 | plt.grid(True) 62 | 63 | plt.subplot(M, 3, 3*i+2) 64 | plt.plot(simX[:, 3*i+1]) 65 | plt.ylabel('vy') 66 | plt.grid(True) 67 | 68 | plt.subplot(M, 3, 3*i+3) 69 | plt.plot(simX[:, 3*i+2]) 70 | plt.ylabel('vz') 71 | plt.grid(True) 72 | 73 | 74 | def plot_chain_control_traj(simU): 75 | plt.figure() 76 | # plt.title('Chain control trajectory, velocities of last mass') 77 | simU = np.vstack((simU[0,:], simU)) 78 | 79 | t = np.array(range(simU.shape[0])) 80 | plt.subplot(3, 1, 1) 81 | plt.step(t, simU[:,0]) 82 | plt.ylabel('vx') 83 | plt.grid(True) 84 | 85 | plt.subplot(3, 1, 2) 86 | plt.step(t, simU[:,1]) 87 | plt.ylabel('vy') 88 | plt.grid(True) 89 | 90 | plt.subplot(3, 1, 3) 91 | plt.step(t, simU[:,2]) 92 | plt.ylabel('vz') 93 | plt.grid(True) 94 | 95 | plt.show() 96 | 97 | 98 | 99 | def plot_chain_position(x, xPosFirstMass): 100 | 101 | if len(x.shape) > 1: 102 | x = x.flatten() 103 | if len(xPosFirstMass.shape) > 1: 104 | xPosFirstMass = xPosFirstMass.flatten() 105 | 106 | nx = x.shape[0] 107 | M = int((nx/3 -1)/2) 108 | 109 | pos = x[:3*(M+1)] 110 | pos = np.hstack((xPosFirstMass, pos)) # append fixed mass 111 | pos_x = pos[::3] 112 | pos_y = pos[1::3] 113 | pos_z = pos[2::3] 114 | 115 | fig = plt.figure() 116 | plt.subplot(3,1,1) 117 | plt.plot(pos_x) 118 | plt.title('x position') 119 | plt.xlabel('mass index ') 120 | plt.ylabel('mass position ') 121 | plt.grid(True) 122 | 123 | plt.subplot(3,1,2) 124 | plt.plot(pos_y) 125 | plt.title('y position') 126 | plt.xlabel('mass index ') 127 | plt.ylabel('mass position ') 128 | plt.grid(True) 129 | 130 | plt.subplot(3,1,3) 131 | plt.plot(pos_z) 132 | plt.title('z position') 133 | plt.xlabel('mass index ') 134 | plt.ylabel('mass position ') 135 | plt.grid(True) 136 | 137 | 138 | def plot_chain_position_3D(X, xPosFirstMass, XNames=None): 139 | """ 140 | X can be either chain state, or tuple of chain states 141 | Xnames is a list of strings 142 | """ 143 | 144 | if not isinstance(X, tuple): 145 | X = (X,) 146 | 147 | if XNames is None: 148 | XNames = [] 149 | for i in range(len(X)): 150 | XNames += ['pos' + str(i + 1)] 151 | 152 | if not isinstance(XNames, list): 153 | XNames = [XNames] 154 | 155 | fig = plt.figure() 156 | ax = fig.gca(projection='3d') 157 | ax.plot(xPosFirstMass[0], xPosFirstMass[1], xPosFirstMass[2], 'rx') 158 | for i, x in enumerate(X): 159 | 160 | if len(x.shape) > 1: 161 | x = x.flatten() 162 | if len(xPosFirstMass.shape) > 1: 163 | xPosFirstMass = xPosFirstMass.flatten() 164 | 165 | nx = x.shape[0] 166 | M = int((nx/3 -1)/2) 167 | pos = x[:3*(M+1)] 168 | pos = np.hstack((xPosFirstMass, pos)) # append fixed mass 169 | pos_x = pos[::3] 170 | pos_y = pos[1::3] 171 | pos_z = pos[2::3] 172 | 173 | ax.plot(pos_x, pos_y, pos_z, '.-', label=XNames[i]) 174 | 175 | ax.set_xlabel('x') 176 | ax.set_ylabel('y') 177 | ax.set_zlabel('z') 178 | plt.legend() 179 | 180 | 181 | def get_plot_lims(a): 182 | 183 | a_min = np.amin(a) 184 | a_max = np.amax(a) 185 | # make sure limits are not equal to each other 186 | eps = 1e-12 187 | if np.abs(a_min - a_max) < eps: 188 | a_min -= 1e-3 189 | a_max += 1e-3 190 | 191 | return (a_min, a_max) 192 | 193 | 194 | def animate_chain_position(simX, xPosFirstMass, Ts=0.1, yPosWall=None): 195 | ''' 196 | Creates animation of the chain, where simX contains the state trajectory. 197 | dt defines the time gap (in seconds) between two succesive entries. 198 | ''' 199 | 200 | # chain positions 201 | Nsim = simX.shape[0] 202 | nx = simX.shape[1] 203 | M = int((nx/3 -1)/2) 204 | pos = simX[:,:3*(M+1)] 205 | 206 | pos_x = np.hstack((xPosFirstMass[0] * np.ones((Nsim,1)), pos[:, ::3])) 207 | pos_y = np.hstack((xPosFirstMass[1] * np.ones((Nsim,1)), pos[:, 1::3])) 208 | pos_z = np.hstack((xPosFirstMass[2] * np.ones((Nsim,1)), pos[:, 2::3])) 209 | 210 | 211 | # limits in all three dimensions 212 | 213 | # ylim_x = (np.amin( pos_x), np.amax( pos_x)) 214 | # ylim_y = (np.amin( pos_y), np.amax( pos_y)) 215 | # ylim_z = (np.amin( pos_z), np.amax( pos_z)) 216 | # eps = 1e-12 217 | # if np.abs(ylim_x[0] - ylim_x[1]) < eps: 218 | # ylim_x[0] += 1e-3 219 | # ylim_x[0] += 1e-3 220 | 221 | ylim_x = get_plot_lims(pos_x) 222 | ylim_y = get_plot_lims(pos_y) 223 | if yPosWall is not None: 224 | ylim_y = (min(ylim_y[0], yPosWall) - 0.1, ylim_y[1]) 225 | ylim_z = get_plot_lims(pos_z) 226 | 227 | fig = plt.figure() 228 | ax1 = fig.add_subplot(311, autoscale_on=False, xlim=(0,M+2), ylim=ylim_x) 229 | plt.grid(True) 230 | ax2 = fig.add_subplot(312, autoscale_on=False, xlim=(0,M+2), ylim=ylim_y) 231 | plt.grid(True) 232 | ax3 = fig.add_subplot(313, autoscale_on=False, xlim=(0,M+2), ylim=ylim_z) 233 | plt.grid(True) 234 | 235 | ax1.set_ylabel('x') 236 | ax2.set_ylabel('y') 237 | ax3.set_ylabel('z') 238 | 239 | # ax.set_aspect('equal') 240 | # ax.axis('off') 241 | 242 | # create empty plot 243 | line1, = ax1.plot([], [], '.-') 244 | line2, = ax2.plot([], [], '.-') 245 | line3, = ax3.plot([], [], '.-') 246 | 247 | lines = [line1, line2, line3] 248 | 249 | if yPosWall is not None: 250 | ax2.plot(yPosWall*np.ones((Nsim,))) 251 | 252 | 253 | def init(): 254 | # placeholder for data 255 | lines = [line1, line2, line3] 256 | for line in lines: 257 | line.set_data([],[]) 258 | 259 | # lines[0].set_data(list(range(M+2)), pos_x[0,:]) 260 | # lines[1].set_data(list(range(M+2)), pos_y[0,:]) 261 | # lines[2].set_data(list(range(M+2)), pos_z[0,:]) 262 | return lines 263 | 264 | 265 | def animate(i): 266 | 267 | lines[0].set_data(list(range(M+2)), pos_x[i,:]) 268 | lines[1].set_data(list(range(M+2)), pos_y[i,:]) 269 | lines[2].set_data(list(range(M+2)), pos_z[i,:]) 270 | 271 | return lines 272 | 273 | ani = animation.FuncAnimation(fig, animate, Nsim, 274 | interval=Ts*1000, repeat_delay=500, 275 | blit=True, init_func=init) 276 | plt.show() 277 | return ani 278 | 279 | 280 | def animate_chain_position_3D(simX, xPosFirstMass, Ts=0.1): 281 | ''' 282 | Create 3D animation of the chain, where simX contains the state trajectory. 283 | dt defines the time gap (in seconds) between two succesive entries. 284 | ''' 285 | 286 | # chain positions 287 | Nsim = simX.shape[0] 288 | nx = simX.shape[1] 289 | M = int((nx/3 -1)/2) 290 | pos = simX[:,:3*(M+1)] 291 | # import pdb; pdb.set_trace() 292 | pos_x = np.hstack((xPosFirstMass[0] * np.ones((Nsim,1)) , pos[:, ::3])) 293 | pos_y = np.hstack((xPosFirstMass[1] * np.ones((Nsim,1)), pos[:, 1::3])) 294 | pos_z = np.hstack((xPosFirstMass[2] * np.ones((Nsim,1)), pos[:, 2::3])) 295 | 296 | 297 | xlim = get_plot_lims(pos_x) 298 | ylim = get_plot_lims(pos_y) 299 | zlim = get_plot_lims(pos_z) 300 | 301 | fig = plt.figure() 302 | ax = fig.add_subplot(111, projection='3d', autoscale_on=False, xlim=xlim, ylim=ylim, zlim=zlim) 303 | 304 | ax.set_xlabel('x') 305 | ax.set_ylabel('y') 306 | ax.set_zlabel('z') 307 | 308 | # ax.set_aspect('equal') 309 | # ax.axis('off') 310 | 311 | # create empty plot 312 | # line, = ax.plot([], [], [], '.-') 313 | line, = ax.plot(pos_x[0,:], pos_y[1,:], pos_z[2,:], '.-') 314 | 315 | def init(): 316 | # placeholder for data 317 | # line.set_data([], []) 318 | # line.set_3d_properties([]) 319 | return line, 320 | 321 | 322 | def animate(i): 323 | 324 | line.set_data(pos_x[i,:], pos_y[i,:]) 325 | # line.set_data(pos_x[i,:], pos_y[i,:], pos_z[i,:]) 326 | line.set_3d_properties(pos_z[i,:]) 327 | return line, 328 | 329 | ani = animation.FuncAnimation(fig, animate, Nsim, 330 | interval=Ts*1000, repeat_delay=500, 331 | blit=True,)# init_func=init) 332 | plt.show() 333 | return ani 334 | 335 | 336 | def timings_plot(timings, n_mass): 337 | # latexify plot 338 | params = get_latex_plot_params() 339 | 340 | matplotlib.rcParams.update(params) 341 | 342 | # actual plot 343 | IDs = timings.keys() 344 | # plot timings 345 | fig = plt.figure(figsize=(6, 3.3)) 346 | ax = plt.gca() 347 | 348 | i_ID = 0 349 | for id in IDs: 350 | timing = timings[id] 351 | # ax = fig.add_subplot(n_subplots, 1, i_ID) 352 | plt.bar(i_ID, np.mean(timings[id]), bottom=np.min(timing)) 353 | plt.plot([i_ID -.5, i_ID + .5], [np.mean(timing), np.mean(timing)], 'k') 354 | # plt.title(id) 355 | i_ID += 1 356 | 357 | ax.set_yscale('log') 358 | plt.xticks(np.arange(len(IDs)), IDs) 359 | plt.ylabel("CPU time per OCP in [s]") 360 | plt.grid(True) 361 | plt.title("CPU times for " + str(n_mass) + " masses") 362 | plt.savefig("figures/timings_nm" + str(n_mass) + ".pdf",\ 363 | bbox_inches='tight', transparent=True, pad_inches=0.05) 364 | plt.show() 365 | 366 | 367 | def nmass_to_nx(n_mass): 368 | M = n_mass - 2 # number of intermediate masses 369 | nx = 6*M + 3 370 | return nx 371 | 372 | 373 | def timings_plot_vary_mass(timings, N_masses): 374 | # latexify plot 375 | params = get_latex_plot_params() 376 | matplotlib.rcParams.update(params) 377 | 378 | # actual plot 379 | IDs = timings.keys() 380 | 381 | fig = plt.figure(figsize=(6, 3.5)) 382 | ax = plt.gca() 383 | 384 | for id in IDs: 385 | timing = timings[id] 386 | mean_time = np.zeros(len(timing.keys())) 387 | 388 | i_nm = 0 389 | for nm in timing.keys(): 390 | mean_time[i_nm] = np.mean(timing[nm]) 391 | i_nm += 1 392 | 393 | print(id, mean_time) 394 | plt.plot(N_masses, mean_time) 395 | 396 | ax.set_yscale('log') 397 | ax.set_xscale('log') 398 | 399 | plt.grid() 400 | 401 | plt.xlabel(r"$n_{\text{mass}}$") 402 | plt.ylabel("mean CPU time per OCP in [s]") 403 | plt.xticks(N_masses, N_masses) 404 | Legends = list(IDs) 405 | Legends = ["naive" if id == "robust" else id for id in Legends] 406 | Legends = ["zoRO-24" if id == "fastzoRO" else id for id in Legends] 407 | Legends = ["zoRO-21" if id == "zoRO" else id for id in Legends] 408 | 409 | # add lines nx^3, nx^6 410 | Legends.append(r"$\mathcal{O}(n_\textrm{x}^{3})$") 411 | plt.plot(N_masses, [4e-6*nmass_to_nx(nm)**3 for nm in N_masses], '--', color="gray") 412 | Legends.append(r"$\mathcal{O}(n_\textrm{x}^{6})$") 413 | plt.plot(N_masses, [1e-7*nmass_to_nx(nm)**6 for nm in N_masses], ':', color="gray") 414 | # Legends.append(r"$n_\textrm{x}^{9}$") 415 | # plt.plot(N_masses, [1e-10*nmass_to_nx(nm)**9 for nm in N_masses], '-.', color="gray") 416 | 417 | plt.legend(Legends, ncol=2) 418 | plt.savefig("figures/timings_vs_nmass" + ".pdf",\ 419 | bbox_inches='tight', transparent=True, pad_inches=0.05) 420 | 421 | plt.show() 422 | 423 | 424 | def constraint_violation_box_plot(violations, n_mass): 425 | # latexify plot 426 | params = get_latex_plot_params() 427 | matplotlib.rcParams.update(params) 428 | 429 | # actual plot 430 | IDs = violations.keys() 431 | 432 | fig = plt.figure() 433 | ax = plt.gca() 434 | 435 | # old: single points 436 | if False: 437 | i_ID = 0 438 | for id in IDs: 439 | violation = violations[id] 440 | plt.plot( i_ID * np.ones((len(violation))), violation, '*') 441 | i_ID += 1 442 | 443 | # new: box plots 444 | else: 445 | data = [] 446 | for id in IDs: 447 | violation = violations[id] 448 | data.append(violation) 449 | ax.boxplot(data, positions = range(len(IDs))) 450 | 451 | plt.plot( [-.5, len(IDs)-.5], [0,0], 'k') 452 | 453 | 454 | plt.ylabel("distance to wall") 455 | plt.xticks(np.arange(len(IDs)), IDs) 456 | 457 | # TODO: boxplot with more values 458 | plt.grid(True) 459 | plt.title("closest distance to wall over scenario") 460 | 461 | # save & show 462 | plt.savefig("figures/constraint_violation_nmass_" + str(n_mass) + "_seeds_" + str(len(violation)) + ".pdf",\ 463 | bbox_inches='tight', transparent=True, pad_inches=0.05) 464 | plt.show() 465 | 466 | return 467 | 468 | 469 | def plot_suboptimility(PS, cost_exact, cost_ZO, title): 470 | # latexify plot 471 | params = get_latex_plot_params() 472 | matplotlib.rcParams.update(params) 473 | 474 | # actual plot 475 | # IDs = violations.keys() 476 | 477 | fig = plt.figure() 478 | ax = plt.gca() 479 | 480 | plt.plot(PS, cost_exact) 481 | plt.plot(PS, cost_ZO) 482 | 483 | suboptimality = np.abs( (np.array(cost_ZO) - np.array(cost_exact)) / np.array(cost_exact)) 484 | plt.plot(PS, suboptimality, '*-') 485 | # print("ZO suboptimality", suboptimality) 486 | 487 | ax.set_xscale('log') 488 | ax.set_yscale('log') 489 | plt.grid(True) 490 | 491 | # plt.plot(cut_PS, [1e7*ps**3 for ps in cut_PS]) 492 | # plt.plot(cut_PS, [1e6*ps**4 for ps in cut_PS]) 493 | 494 | Legends = ["exact robust", "ZO robust", "abs difference", "ps**3", "ps**4"] 495 | # Legends = ["suboptimality ZO wrt exact robust"] 496 | plt.legend(Legends) 497 | 498 | plt.xlabel("pertubation scale") 499 | plt.ylabel("relative suboptimality") 500 | 501 | # plt.xlim([0, 0.1]) 502 | # plt.ylim([0, 0.002]) 503 | 504 | plt.title(title) 505 | 506 | return -------------------------------------------------------------------------------- /experiment_suboptimality.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy as np 3 | import scipy.linalg 4 | 5 | from time import process_time 6 | 7 | from acados_template import AcadosOcp, AcadosOcpSolver, AcadosSimSolver 8 | 9 | from export_chain_mass_model import export_chain_mass_model 10 | from export_disturbed_chain_mass_model import export_disturbed_chain_mass_model 11 | from export_chain_mass_integrator import export_chain_mass_integrator 12 | from export_augmented_chain_mass_model import export_augmented_chain_mass_model 13 | 14 | from plot_utils import * 15 | from utils import * 16 | import matplotlib.pyplot as plt 17 | 18 | def ZO_robust(chain_params): 19 | # create ocp object to formulate the OCP 20 | ocp = AcadosOcp() 21 | 22 | # chain parameters 23 | n_mass = chain_params["n_mass"] 24 | M = chain_params["n_mass"] - 2 # number of intermediate masses 25 | Ts = chain_params["Ts"] 26 | Tsim = chain_params["Tsim"] 27 | N = chain_params["N"] 28 | u_init = chain_params["u_init"] 29 | with_wall = chain_params["with_wall"] 30 | yPosWall = chain_params["yPosWall"] 31 | m = chain_params["m"] 32 | D = chain_params["D"] 33 | L = chain_params["L"] 34 | perturb_scale = chain_params["perturb_scale"] 35 | 36 | nlp_iter = chain_params["nlp_iter"] 37 | nlp_tol = chain_params["nlp_tol"] 38 | save_results = chain_params["save_results"] 39 | show_plots = chain_params["show_plots"] 40 | seed = chain_params["seed"] 41 | 42 | np.random.seed(seed) 43 | 44 | nparam = 3*M 45 | 46 | # experiment adjustments 47 | # no closed loop 48 | Tsim = Ts 49 | 50 | W = perturb_scale * np.eye(nparam) 51 | 52 | # export model 53 | model = export_disturbed_chain_mass_model(n_mass, m, D, L) 54 | model.name = model.name + "_robust" 55 | 56 | # set model 57 | ocp.model = model 58 | 59 | nx = model.x.size()[0] 60 | nu = model.u.size()[0] 61 | ny = nx + nu 62 | ny_e = nx 63 | Tf = N * Ts 64 | 65 | # initial state 66 | xPosFirstMass = np.zeros((3,1)) 67 | xEndRef = np.zeros((3,1)) 68 | xEndRef[0] = L * (M+1) * 6 69 | pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass) 70 | 71 | xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef) 72 | 73 | x0 = xrest 74 | 75 | # set dimensions 76 | ocp.solver_options.N_horizon = N 77 | 78 | # set cost module 79 | ocp.cost.cost_type = 'LINEAR_LS' 80 | ocp.cost.cost_type_e = 'LINEAR_LS' 81 | 82 | Q = 2*np.diagflat( np.ones((nx, 1)) ) 83 | q_diag = np.ones((nx,1)) 84 | strong_penalty = M+1 85 | q_diag[3*M] = strong_penalty 86 | q_diag[3*M+1] = strong_penalty 87 | q_diag[3*M+2] = strong_penalty 88 | Q = 2*np.diagflat( q_diag ) 89 | 90 | R = 2*np.diagflat( 1e-2 * np.ones((nu, 1)) ) 91 | 92 | ocp.cost.W = scipy.linalg.block_diag(Q, R) 93 | ocp.cost.W_e = Q 94 | 95 | ocp.cost.Vx = np.zeros((ny, nx)) 96 | ocp.cost.Vx[:nx,:nx] = np.eye(nx) 97 | 98 | Vu = np.zeros((ny, nu)) 99 | Vu[nx:nx+nu, :] = np.eye(nu) 100 | ocp.cost.Vu = Vu 101 | 102 | ocp.cost.Vx_e = np.eye(nx) 103 | 104 | # import pdb; pdb.set_trace() 105 | yref = np.vstack((xrest, np.zeros((nu,1)))).flatten() 106 | ocp.cost.yref = yref 107 | ocp.cost.yref_e = xrest.flatten() 108 | 109 | # set constraints 110 | umax = 1*np.ones((nu,)) 111 | 112 | ocp.constraints.constr_type = 'BGH' 113 | ocp.constraints.lbu = -umax 114 | ocp.constraints.ubu = umax 115 | ocp.constraints.x0 = x0.reshape((nx,)) 116 | ocp.constraints.idxbu = np.array(range(nu)) 117 | 118 | # disturbances 119 | nparam = 3*M 120 | ocp.parameter_values = np.zeros((nparam,)) 121 | 122 | # wall constraint 123 | if with_wall: 124 | nbx = M + 1 125 | Jbx = np.zeros((nbx,nx)) 126 | for i in range(nbx): 127 | Jbx[i, 3*i+1] = 1.0 128 | 129 | ocp.constraints.Jbx = Jbx 130 | ocp.constraints.lbx = yPosWall * np.ones((nbx,)) 131 | ocp.constraints.ubx = 1e9 * np.ones((nbx,)) 132 | 133 | # slacks 134 | # ocp.constraints.Jsbx = np.eye(nbx) 135 | # L2_pen = 1e3 136 | # L1_pen = 1 137 | # ocp.cost.Zl = L2_pen * np.ones((nbx,)) 138 | # ocp.cost.Zu = L2_pen * np.ones((nbx,)) 139 | # ocp.cost.zl = L1_pen * np.ones((nbx,)) 140 | # ocp.cost.zu = L1_pen * np.ones((nbx,)) 141 | 142 | 143 | # solver options 144 | ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES 145 | ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' 146 | ocp.solver_options.integrator_type = 'IRK' 147 | ocp.solver_options.nlp_solver_type = 'SQP_RTI' # SQP, SQP_RTI 148 | 149 | ocp.solver_options.sim_method_num_stages = 2 150 | ocp.solver_options.sim_method_num_steps = 2 151 | ocp.solver_options.qp_solver_cond_N = N 152 | ocp.solver_options.tol = nlp_tol 153 | ocp.solver_options.qp_tol = nlp_tol 154 | ocp.solver_options.nlp_solver_max_iter = nlp_iter 155 | # ocp.solver_options.qp_solver_iter_max = 500 156 | # ocp.solver_options.qp_solver_max_iter = 100 157 | 158 | # set prediction horizon 159 | ocp.solver_options.tf = Tf 160 | 161 | 162 | # acados_integrator = AcadosSimSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 163 | acados_integrator = export_chain_mass_integrator(n_mass, m, D, L) 164 | 165 | acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 166 | 167 | #%% get initial state from xrest 168 | xcurrent = x0.reshape((nx,)) 169 | for i in range(5): 170 | acados_integrator.set("x", xcurrent) 171 | acados_integrator.set("u", u_init) 172 | 173 | status = acados_integrator.solve() 174 | if status != 0: 175 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 176 | 177 | # update state 178 | xcurrent = acados_integrator.get("x") 179 | 180 | #%% actual simulation 181 | N_sim = int(np.floor(Tsim/Ts)) 182 | simX = np.ndarray((N_sim+1, nx)) 183 | simU = np.ndarray((N_sim, nu)) 184 | wall_dist = np.zeros((N_sim,)) 185 | 186 | simX[0,:] = xcurrent 187 | 188 | P0_mat = 1e-3 * np.eye(nx) 189 | P_mat_list = [None] * (N+1) 190 | P_mat_list[0] = P0_mat 191 | 192 | nbx = M + 1 193 | lbx = np.zeros((nbx,)) 194 | 195 | # solve ocp 196 | acados_ocp_solver.set(0, "lbx", xcurrent) 197 | acados_ocp_solver.set(0, "ubx", xcurrent) 198 | 199 | if ocp.solver_options.nlp_solver_type == 'SQP_RTI': 200 | # SQP loop: 201 | for i_sqp in range(ocp.solver_options.nlp_solver_max_iter): 202 | 203 | # preparation rti_phase 204 | acados_ocp_solver.options_set('rti_phase', 1) 205 | status = acados_ocp_solver.solve() 206 | 207 | # hardcode B for discrete time disturbance 208 | B = np.vstack(( np.zeros((nx - nparam, nparam)), np.eye(nparam))) 209 | for stage in range(N): 210 | # get A matrices 211 | A = acados_ocp_solver.get_from_qp_in(stage, "A") 212 | # propagate Ps 213 | 214 | P_mat_list[stage+1] = P_propagation(P_mat_list[stage], A, B, W*Ts) 215 | 216 | # compute backoff using P 217 | for j in range(nbx): 218 | P = P_mat_list[stage] 219 | lbx[j] = yPosWall + np.sqrt(P[3*j+1,3*j+1]) 220 | 221 | # set bounds with backoff (nabla h available since h linear) 222 | if stage > 0: 223 | acados_ocp_solver.constraints_set(stage, "lbx", lbx) 224 | 225 | # - h <-> wall constraint 226 | 227 | # feedback rti_phase 228 | acados_ocp_solver.options_set('rti_phase', 2) 229 | status = acados_ocp_solver.solve() 230 | 231 | # check on residuals and terminate loop. 232 | # acados_ocp_solver.print_statistics() # encapsulates: stat = acados_ocp_solver.get_stats("statistics") 233 | residuals = acados_ocp_solver.get_residuals() 234 | print("residuals after ", i_sqp, "SQP_RTI iterations:\n", residuals) 235 | 236 | if status != 0: 237 | raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i)) 238 | 239 | if max(residuals) < nlp_tol: 240 | break 241 | 242 | else: 243 | status = acados_ocp_solver.solve() 244 | # acados_ocp_solver.print_statistics() # encapsulates: stat = acados_ocp_solver.get_stats("statistics") 245 | if status != 0: 246 | raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i)) 247 | 248 | simU[0,:] = acados_ocp_solver.get(0, "u") 249 | 250 | # only LQR 251 | # simU[i,:] = K @ (xcurrent - xrest.flatten()) 252 | 253 | print("control at time", 0, ":", simU[0,:]) 254 | 255 | i = 0 256 | # simulate system 257 | acados_integrator.set("x", xcurrent) 258 | acados_integrator.set("u", simU[0,:]) 259 | 260 | pertubation = sampleFromEllipsoid(np.zeros((nparam,)), W) 261 | # acados_integrator.set("p", pertubation) 262 | 263 | status = acados_integrator.solve() 264 | if status != 0: 265 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 266 | 267 | # update state 268 | xcurrent = acados_integrator.get("x") + Ts * np.hstack((np.zeros(((M+1)*3,)), pertubation)) 269 | simX[i+1,:] = xcurrent 270 | 271 | # xOcpPredict = acados_ocp_solver.get(1, "x") 272 | # print("model mismatch = ", str(np.max(xOcpPredict - xcurrent))) 273 | yPos = xcurrent[range(1,3*M+2,3)] 274 | wall_dist[i] = np.min(yPos - yPosWall) 275 | print("time i = ", str(i), " dist2wall ", str(wall_dist[i])) 276 | 277 | # get cost function value 278 | cost = acados_ocp_solver.get_cost() 279 | print("cost function value of solution = ", cost) 280 | 281 | # compute cost ourselves - without soft constraint contribution 282 | cost2 = 0 283 | for j in range(N): 284 | xocp = acados_ocp_solver.get(j, "x") 285 | uocp = acados_ocp_solver.get(j, "u") 286 | 287 | dx = xocp[:nx] - xrest.flatten() 288 | cost2 += dx.T @ Q @ dx + uocp.T @ R @ uocp 289 | 290 | xocp = acados_ocp_solver.get(N, "x") 291 | dx = xocp[:nx] - xrest.flatten() 292 | cost2 += dx.T @ Q @ dx 293 | 294 | return cost, cost2 295 | 296 | 297 | def naive_robust(chain_params): 298 | # create ocp object to formulate the OCP 299 | ocp = AcadosOcp() 300 | 301 | # chain parameters 302 | n_mass = chain_params["n_mass"] 303 | M = chain_params["n_mass"] - 2 # number of intermediate masses 304 | Ts = chain_params["Ts"] 305 | Tsim = chain_params["Tsim"] 306 | N = chain_params["N"] 307 | u_init = chain_params["u_init"] 308 | with_wall = chain_params["with_wall"] 309 | yPosWall = chain_params["yPosWall"] 310 | m = chain_params["m"] 311 | D = chain_params["D"] 312 | L = chain_params["L"] 313 | perturb_scale = chain_params["perturb_scale"] 314 | 315 | nlp_iter = chain_params["nlp_iter"] 316 | nlp_tol = chain_params["nlp_tol"] 317 | save_results = chain_params["save_results"] 318 | show_plots = chain_params["show_plots"] 319 | seed = chain_params["seed"] 320 | 321 | np.random.seed(seed) 322 | 323 | nparam = 3*M 324 | W = perturb_scale * np.eye(nparam) 325 | 326 | # export model 327 | model = export_augmented_chain_mass_model(n_mass, m, D, L, Ts, W) 328 | 329 | # set model 330 | ocp.model = model 331 | 332 | nx_orig = M * 3 + (M+1)*3 333 | nx_aug = model.x.shape[0] 334 | 335 | nu = model.u.size()[0] 336 | ny = nx_orig + nu 337 | ny_e = nx_orig 338 | Tf = N * Ts 339 | 340 | # initial state 341 | xPosFirstMass = np.zeros((3,1)) 342 | xEndRef = np.zeros((3,1)) 343 | xEndRef[0] = L * (M+1) * 6 344 | pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass) 345 | 346 | xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef) 347 | 348 | x0 = xrest 349 | 350 | P0_mat = 1e-3 * np.eye(nx_orig) 351 | P0_vec = sym_mat2vec(P0_mat) 352 | 353 | # set dimensions 354 | ocp.solver_options.N_horizon = N 355 | 356 | # set cost module 357 | ocp.cost.cost_type = 'LINEAR_LS' 358 | ocp.cost.cost_type_e = 'LINEAR_LS' 359 | 360 | Q = 2*np.diagflat( np.ones((nx_orig, 1)) ) 361 | q_diag = np.ones((nx_orig,1)) 362 | strong_penalty = M+1 363 | q_diag[3*M] = strong_penalty 364 | q_diag[3*M+1] = strong_penalty 365 | q_diag[3*M+2] = strong_penalty 366 | Q = 2*np.diagflat( q_diag ) 367 | 368 | R = 2*np.diagflat( 1e-2 * np.ones((nu, 1)) ) 369 | 370 | ocp.cost.W = scipy.linalg.block_diag(Q, R) 371 | ocp.cost.W_e = Q 372 | 373 | ocp.cost.Vx = np.zeros((ny, nx_aug)) 374 | ocp.cost.Vx[:nx_orig,:nx_orig] = np.eye(nx_orig) 375 | 376 | Vu = np.zeros((ny, nu)) 377 | Vu[nx_orig:nx_orig+nu, :] = np.eye(nu) 378 | ocp.cost.Vu = Vu 379 | 380 | Vx_e = np.zeros((ny_e, nx_aug)) 381 | Vx_e[:nx_orig, :nx_orig] = np.eye(nx_orig) 382 | ocp.cost.Vx_e = Vx_e 383 | 384 | 385 | yref = np.vstack((xrest, np.zeros((nu,1)))).flatten() 386 | ocp.cost.yref = yref 387 | ocp.cost.yref_e = xrest.flatten() 388 | 389 | # set constraints 390 | umax = 1*np.ones((nu,)) 391 | 392 | ocp.constraints.constr_type = 'BGH' 393 | ocp.constraints.lbu = -umax 394 | ocp.constraints.ubu = umax 395 | x0_aug = np.hstack((x0.flatten(), P0_vec)) 396 | ocp.constraints.x0 = x0_aug.reshape((nx_aug,)) 397 | ocp.constraints.idxbu = np.array(range(nu)) 398 | 399 | # disturbances 400 | 401 | # wall constraint 402 | if with_wall: 403 | # slacks 404 | L2_pen = 1e3 405 | L1_pen = 1 406 | 407 | nh = M + 1 408 | 409 | ocp.constraints.lh = yPosWall * np.ones((nh,)) 410 | ocp.constraints.uh = 1e9 * np.ones((nh,)) 411 | # ocp.constraints.Jsh = np.eye(nh) 412 | h_expr = ca.SX.zeros(nh,1) 413 | for j in range(nh): 414 | P_mat = vec2sym_mat( model.x[nx_orig:], nx_orig ) 415 | # Note: lower bound, therefore need to substract the backoff term 416 | h_expr[j] = model.x[3*j+1] - ca.sqrt(P_mat[3*j+1,3*j+1]) 417 | ocp.model.con_h_expr = h_expr 418 | 419 | # ocp.cost.Zl = L2_pen * np.ones((nh,)) 420 | # ocp.cost.Zu = L2_pen * np.ones((nh,)) 421 | # ocp.cost.zl = L1_pen * np.ones((nh,)) 422 | # ocp.cost.zu = L1_pen * np.ones((nh,)) 423 | 424 | 425 | 426 | # solver options 427 | ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES 428 | # ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' # FULL_CONDENSING_QPOASES 429 | 430 | ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' 431 | ocp.solver_options.integrator_type = 'IRK' 432 | ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI 433 | ocp.solver_options.nlp_solver_max_iter = nlp_iter 434 | 435 | ocp.solver_options.sim_method_num_stages = 2 436 | ocp.solver_options.sim_method_num_steps = 2 437 | ocp.solver_options.qp_solver_cond_N = N # N TODO 438 | ocp.solver_options.tol = nlp_tol 439 | ocp.solver_options.qp_tol = nlp_tol 440 | ocp.solver_options.print_level = 0 441 | 442 | # set prediction horizon 443 | ocp.solver_options.tf = Tf 444 | acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 445 | print("Generated AcadosOcpSolver successfully") 446 | # acados_integrator = AcadosSimSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') 447 | acados_integrator = export_chain_mass_integrator(n_mass, m, D, L) 448 | 449 | #%% get initial state from xrest 450 | xcurrent = x0.reshape((nx_orig,)) 451 | for i in range(5): 452 | acados_integrator.set("x", xcurrent) 453 | acados_integrator.set("u", u_init) 454 | 455 | status = acados_integrator.solve() 456 | if status != 0: 457 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 458 | 459 | # update state 460 | xcurrent = acados_integrator.get("x") 461 | 462 | #%% actual simulation 463 | N_sim = int(np.floor(Tsim/Ts)) 464 | simX = np.ndarray((N_sim+1, nx_orig)) 465 | simU = np.ndarray((N_sim, nu)) 466 | wall_dist = np.zeros((N_sim,)) 467 | simX[0,:] = xcurrent 468 | 469 | timings = np.zeros((N_sim,)) 470 | Pposdef = np.zeros((N_sim, N)) 471 | 472 | xcurrent_aug = np.hstack((xcurrent.flatten(), P0_vec)) 473 | 474 | # no closed loop 475 | i = 0 476 | # solve ocp 477 | acados_ocp_solver.set(0, "lbx", xcurrent_aug) 478 | acados_ocp_solver.set(0, "ubx", xcurrent_aug) 479 | 480 | status = acados_ocp_solver.solve() 481 | acados_ocp_solver.print_statistics() 482 | 483 | if status != 0: 484 | raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i)) 485 | 486 | simU[i,:] = acados_ocp_solver.get(0, "u") 487 | 488 | # simulate system 489 | acados_integrator.set("x", xcurrent) 490 | acados_integrator.set("u", simU[i,:]) 491 | 492 | pertubation = sampleFromEllipsoid(np.zeros((nparam,)), W) 493 | acados_integrator.set("p", pertubation) 494 | 495 | status = acados_integrator.solve() 496 | if status != 0: 497 | raise Exception('acados integrator returned status {}. Exiting.'.format(status)) 498 | 499 | # update state 500 | xcurrent = acados_integrator.get("x") 501 | simX[i+1,:] = xcurrent 502 | xcurrent_aug = np.hstack((xcurrent.flatten(), P0_vec)) 503 | 504 | # get P covariances 505 | for j in range(N): 506 | xocp = acados_ocp_solver.get(j, "x") 507 | P = vec2sym_mat(xocp[nx_orig:], nx_orig) 508 | Pposdef[i,j] = is_pos_def(P) 509 | 510 | timings[i] = acados_ocp_solver.get_stats("time_tot")[0] 511 | 512 | yPos = xcurrent[range(1,3*M+1,3)] 513 | wall_dist[i] = np.min(yPos - yPosWall) 514 | print("time i = ", str(i), " dist2wall ", str(wall_dist[i])) 515 | 516 | 517 | # get cost function value 518 | cost = acados_ocp_solver.get_cost() 519 | print("cost function value of solution = ", cost) 520 | 521 | # compute cost ourselves - without soft constraint contribution 522 | cost2 = 0 523 | for j in range(N): 524 | xocp = acados_ocp_solver.get(j, "x") 525 | uocp = acados_ocp_solver.get(j, "u") 526 | 527 | dx = xocp[:nx_orig] - xrest.flatten() 528 | cost2 += dx.T @ Q @ dx + uocp.T @ R @ uocp 529 | 530 | xocp = acados_ocp_solver.get(N, "x") 531 | dx = xocp[:nx_orig] - xrest.flatten() 532 | cost2 += dx.T @ Q @ dx 533 | 534 | return cost, cost2 535 | 536 | 537 | # MAIN 538 | chain_params = get_chain_params() 539 | 540 | 541 | # min_ps = -3 542 | # max_ps = 0 543 | # n_pert = 30 544 | 545 | min_ps = -6 546 | max_ps = -1 547 | n_pert = 30 548 | 549 | PS = np.logspace(min_ps, max_ps, n_pert) 550 | i_pert = 0 551 | 552 | cost_ZO = np.zeros((n_pert,1)) 553 | cost_exact = np.zeros((n_pert,1)) 554 | cost2_ZO = np.zeros((n_pert,1)) 555 | cost2_exact = np.zeros((n_pert,1)) 556 | chain_params["nlp_tol"] = 1e-6 557 | 558 | for perturb_scale in PS: 559 | chain_params["perturb_scale"] = perturb_scale 560 | cost_ZO[i_pert], cost2_ZO[i_pert] = ZO_robust(chain_params) 561 | # cost, cost2 = ZO_robust(chain_params) 562 | # import pdb; pdb.set_trace() 563 | 564 | cost_exact[i_pert], cost2_exact[i_pert] = naive_robust(chain_params) 565 | i_pert += 1 566 | 567 | 568 | results = dict() 569 | results["pertubation_scaling"] = PS 570 | results["cost_ZO"] = cost_ZO 571 | results["cost_exact"] = cost_exact 572 | results["cost2_ZO"] = cost2_ZO 573 | results["cost2_exact"] = cost2_exact 574 | 575 | 576 | # json_file = "results/suboptimality" + str(n_pert) + ".json" 577 | json_file = "results/suboptimality" + str(min_ps) + "_" + str(max_ps) + "_" + str(n_pert) + ".json" 578 | json_file = "results/suboptimality_high_acc" + str(min_ps) + "_" + str(max_ps) + "_" + str(n_pert) + ".json" 579 | 580 | save_results_as_json(results, json_file) 581 | 582 | plot_suboptimility(PS, cost_exact, cost_ZO, "acados cost") 583 | --------------------------------------------------------------------------------