# [00, 00, @2, 00, 00], # [00, 00, 00, @1, 00], # [00, 00, 00, 00, @1]]) # NOTE: hessian is wrt [u,x] if EXTERNAL_COST_USE_NUM_HESS: for i in range(N): ocp_solver.cost_set(i, "ext_cost_num_hess", np.diag([0.04, 4000, 4000, 0.04, 0.04, ])) ocp_solver.cost_set(N, "ext_cost_num_hess", np.diag([4000, 4000, 0.04, 0.04, ])) simX = np.ndarray((N+1, nx)) simU = np.ndarray((N, nu)) status = ocp_solver.solve() ocp_solver.print_statistics() if status != 0: raise Exception('acados returned status {}. Exiting.'.format(status)) # get solution for i in range(N): simX[i,:] = ocp_solver.get(i, "x") simU[i,:] = ocp_solver.get(i, "u") simX[N,:] = ocp_solver.get(N, "x") plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False)
simX[N, :] = ocp_solver.get(N, "x") print("inequality multipliers at stage 1") print(ocp_solver.get(1, "lam")) # inequality multipliers at stage 1 print("slack values at stage 1") print(ocp_solver.get(1, "t")) # slack values at stage 1 print("multipliers of dynamic conditions between stage 1 and 2") print(ocp_solver.get( 1, "pi")) # multipliers of dynamic conditions between stage 1 and 2 # initialize ineq multipliers and slacks at stage 1 ocp_solver.set(1, "lam", np.zeros(2, )) ocp_solver.set(1, "t", np.zeros(2, )) ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") # timings time_tot = ocp_solver.get_stats("time_tot") time_lin = ocp_solver.get_stats("time_lin") time_sim = ocp_solver.get_stats("time_sim") time_qp = ocp_solver.get_stats("time_qp") print( f"timings OCP solver: total: {1e3*time_tot}ms, lin: {1e3*time_lin}ms, sim: {1e3*time_sim}ms, qp: {1e3*time_qp}ms" ) # print("simU", simU) # print("simX", simX) plot_pendulum(shooting_nodes, Fmax, simU, simX, latexify=False)
# else: # Exception("Failed to detect GNSF structure in Octave") # load gnsf from json with open(model.name + '_gnsf_functions.json', 'r') as f: import json gnsf_dict = json.load(f) ocp.gnsf_model = gnsf_dict # set prediction horizon ocp.solver_options.tf = Tf ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json') simX = np.ndarray((N + 1, nx)) simU = np.ndarray((N, nu)) status = ocp_solver.solve() if status != 0: raise Exception('acados returned status {}. Exiting.'.format(status)) # get solution for i in range(N): simX[i, :] = ocp_solver.get(i, "x") simU[i, :] = ocp_solver.get(i, "u") simX[N, :] = ocp_solver.get(N, "x") plot_pendulum(np.linspace(0, Tf, N + 1), Fmax, simU, simX)
# set simulation time sim.solver_options.T = Tf # set options sim.solver_options.num_stages = 4 sim.solver_options.num_steps = 3 sim.solver_options.newton_iter = 3 # for implicit integrator # create acados_integrator = AcadosSimSolver(sim) simX = np.ndarray((N + 1, nx)) x0 = np.array([0.0, np.pi + 1, 0.0, 0.0]) u0 = np.array([0.0]) acados_integrator.set("u", u0) simX[0, :] = x0 for i in range(N): # set initial state acados_integrator.set("x", simX[i, :]) # solve status = acados_integrator.solve() # get solution simX[i + 1, :] = acados_integrator.get("x") if status != 0: raise Exception('acados returned status {}. Exiting.'.format(status)) # plot results plot_pendulum(Tf / N, 10, np.zeros((N, nu)), simX)
def run_closed_loop_experiment(FORMULATION): # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = export_pendulum_ode_model() ocp.model = model Tf = 1.0 nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx N = 20 # set dimensions ocp.dims.nx = nx ocp.dims.ny = ny ocp.dims.ny_e = ny_e ocp.dims.nu = model.u.size()[0] ocp.dims.ns = nu ocp.dims.N = N ocp.dims.nbu = 1 if FORMULATION == 0: ocp.dims.nh = 0 ocp.dims.nsh = 0 ocp.dims.nbx = 1 ocp.dims.nsbx = 1 elif FORMULATION == 1: ocp.dims.nh = 1 ocp.dims.nsh = 1 ocp.dims.nbx = 0 ocp.dims.nsbx = 0 # set cost module ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2]) R = 2 * np.diag([1e-2]) ocp.cost.W = scipy.linalg.block_diag(Q, R) ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) Vu = np.zeros((ny, nu)) Vu[4, 0] = 1.0 ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) ocp.cost.W_e = Q ocp.cost.yref = np.zeros((ny, )) ocp.cost.yref_e = np.zeros((ny_e, )) ocp.cost.zl = 2000 * np.ones((1, )) ocp.cost.Zl = 1 * np.ones((1, )) ocp.cost.zu = 2000 * np.ones((1, )) ocp.cost.Zu = 1 * np.ones((1, )) # set constraints Fmax = 80 vmax = 5 x0 = np.array([0.0, np.pi, 0.0, 0.0]) ocp.constraints.x0 = x0 # bound on u ocp.constraints.lbu = np.array([-Fmax]) ocp.constraints.ubu = np.array([+Fmax]) ocp.constraints.idxbu = np.array([0]) if FORMULATION == 0: # soft bound on x ocp.constraints.lbx = np.array([-vmax]) ocp.constraints.ubx = np.array([+vmax]) ocp.constraints.idxbx = np.array([2]) # v is x[2] # indices of slacked constraints within bx ocp.constraints.idxsbx = np.array([0]) # bounds on slack variables ocp.constraints.lsbx = np.zeros((ocp.dims.nsbx, )) ocp.constraints.usbx = np.zeros((ocp.dims.nsbx, )) elif FORMULATION == 1: # soft bound on x, using constraint h v1 = ocp.model.x[2] ocp.model.con_h_expr = v1 ocp.constraints.lh = np.array([-vmax]) ocp.constraints.uh = np.array([+vmax]) # indices of slacked constraints within h ocp.constraints.idxsh = np.array([0]) # bounds on slack variables ocp.constraints.lsh = np.zeros((ocp.dims.nh, )) ocp.constraints.ush = np.zeros((ocp.dims.nh, )) # set options ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.tf = Tf ocp.solver_options.nlp_solver_type = 'SQP' json_filename = 'pendulum_soft_constraints.json' acados_ocp_solver = AcadosOcpSolver(ocp, json_file=json_filename) acados_integrator = AcadosSimSolver(ocp, json_file=json_filename) # closed loop Nsim = 20 simX = np.ndarray((Nsim + 1, nx)) simU = np.ndarray((Nsim, nu)) xcurrent = x0 for i in range(Nsim): simX[i, :] = xcurrent # solve ocp acados_ocp_solver.set(0, "lbx", xcurrent) acados_ocp_solver.set(0, "ubx", xcurrent) status = acados_ocp_solver.solve() if status != 0: raise Exception( 'acados acados_ocp_solver returned status {}. Exiting.'.format( status)) simU[i, :] = acados_ocp_solver.get(0, "u") # simulate system acados_integrator.set("x", xcurrent) acados_integrator.set("u", simU[i, :]) status = acados_integrator.solve() if status != 0: raise Exception( 'acados integrator returned status {}. Exiting.'.format( status)) # update state xcurrent = acados_integrator.get("x") simX[Nsim, :] = xcurrent # plot results plot_pendulum(Tf / N, Fmax, simU, simX, latexify=False) # store results np.savetxt('test_results/simX_soft_formulation_' + str(FORMULATION), simX) np.savetxt('test_results/simU_soft_formulation_' + str(FORMULATION), simU) print("soft constraint example: ran formulation", FORMULATION, "successfully.")
if status != 0: raise Exception('acados returned status {}. Exiting.'.format(status)) # get solution for i in range(N): x_augmented = acados_solver_mhe.get(i, "x") simXest[i, :] = x_augmented[0:nx] simMest[i, :] = x_augmented[nx] simWest[i, :] = acados_solver_mhe.get(i, "u") x_augmented = acados_solver_mhe.get(N, "x") simXest[N, :] = x_augmented[0:nx] simMest[N, :] = x_augmented[nx] print('difference |x0_est - x0_bar|', np.linalg.norm(x0_bar[0:nx] - simXest[0, :])) print('difference |x_est - x_true|', np.linalg.norm(simXest - simX)) print('difference |M_est - M_true|', np.abs(simMest[0] - M_true)) ts = np.linspace(0, Tf, N + 1) plot_pendulum(ts, Fmax, simU, simX, simXest, simY, latexify=False) # import matplotlib.pyplot as plt # plt.figure() # plt.plot(ts, M_true*np.ones((N+1, 1)), '-') # plt.plot(ts, simMest, '.-') # plt.grid() # plt.ylabel('M') # plt.legend(['true M', 'estimated M']) # plt.show()
raise Exception('acados returned status {}. Exiting.'.format(status)) # get solution for i in range(N0): simX0[i, :] = ocp_solver.get(i, "x") simU0[i, :] = ocp_solver.get(i, "u") simX0[N0, :] = ocp_solver.get(N0, "x") ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") # plot but don't halt plot_pendulum(np.linspace(0, Tf_01, N0 + 1), Fmax, simU0, simX0, latexify=False, plt_show=False, X_true_label=f'original: N={N0}, Tf={Tf_01}') # -------------------------------------------------------------------------------- # 1) now reuse the code but set a new time-steps vector, with a new number of elements dt1 = Tf_01 / N12 new_time_steps1 = np.tile(dt1, (N12, )) # Matlab's equivalent to repmat time1 = np.hstack([0, np.cumsum(new_time_steps1)]) simX1 = np.ndarray((N12 + 1, nx)) simU1 = np.ndarray((N12, nu)) ocp_solver.set_new_time_steps(new_time_steps1)
ocp.constraints.ubu = np.array([+Fmax]) ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) ocp.constraints.idxbu = np.array([0]) ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0 # set prediction horizon ocp.solver_options.tf = Tf ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') simX = np.ndarray((N+1, nx)) simU = np.ndarray((N, nu)) status = ocp_solver.solve() if status != 0: raise Exception('acados returned status {}. Exiting.'.format(status)) # get solution for i in range(N): simX[i,:] = ocp_solver.get(i, "x") simU[i,:] = ocp_solver.get(i, "u") simX[N,:] = ocp_solver.get(N, "x") plot_pendulum(Tf/N, Fmax, simU, simX)
status = acados_solver_ocp.solve() # acados_solver_ocp.print_statistics() if status != 0: raise Exception('controller returned status {} in step {}. Exiting.'.format(status, i)) simU[i:, ] = acados_solver_ocp.get(0, "u") ### simulation ### # measurement simY[i,:] = simX[i, :] + (V @ np.random.standard_normal((nx,1))).T w = W @ np.random.standard_normal((nx,)) plant.set("u", w) plant.set("p", simU[i,:]) plant.set("x", simX[i,:]) status = plant.solve() if status != 0: raise Exception('integrator returned status {} in step {}. Exiting.'.format(status, i)) simX[i+1,:] = plant.get("x") # plot print('estimation error p', np.linalg.norm(simX[:, 0] - simXest[:, 0])) print('estimation error theta', np.linalg.norm(simX[:, 1] - simXest[:, 1])) print('estimation error v', np.linalg.norm(simX[:, 2] - simXest[:, 2])) print('estimation error dtheta', np.linalg.norm(simX[:, 3] - simXest[:, 3])) plot_pendulum(np.linspace(0, Ts*Nsim, Nsim+1), u_max, simU, simX, simXest[N_mhe:, :], simY)
simX[0,:] = x0 acados_integrator.set("S_adj", np.ones((nx+nu, 1))) for i in range(N): # set initial state acados_integrator.set("x", simX[i,:]) # solve status = acados_integrator.solve() # get solution simX[i+1,:] = acados_integrator.get("x") if status != 0: raise Exception(f'acados returned status {status}.') S_forw = acados_integrator.get("S_forw") Sx = acados_integrator.get("Sx") Su = acados_integrator.get("Su") S_hess = acados_integrator.get("S_hess") S_adj = acados_integrator.get("S_adj") print("S_forw, sensitivities of simulaition result wrt x,u:\n", S_forw) print("Sx, sensitivities of simulaition result wrt x:\n", Sx) print("Su, sensitivities of simulaition result wrt u:\n", Su) print("S_adj, adjoint sensitivities:\n", S_adj) print("S_hess, second order sensitivities:\n", S_hess) # plot results plot_pendulum(np.linspace(0, Tf, N+1), 10, np.zeros((N, nu)), simX, latexify=False)
ocp.constraints.idxbu = np.array([0]) ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = HESSIAN_APPROXIMATION ocp.solver_options.regularize_method = 'CONVEXIFY' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.qp_solver_cond_N = 5 # set prediction horizon ocp.solver_options.tf = Tf ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json') simX = np.ndarray((N + 1, nx)) simU = np.ndarray((N, nu)) status = ocp_solver.solve() if status != 0: raise Exception('acados returned status {}. Exiting.'.format(status)) # get solution for i in range(N): simX[i, :] = ocp_solver.get(i, "x") simU[i, :] = ocp_solver.get(i, "u") simX[N, :] = ocp_solver.get(N, "x") plot_pendulum(Tf / N, Fmax, simU, simX, latexify=False)
def main(interface_type='ctypes'): # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = export_pendulum_ode_model() ocp.model = model nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx # define the different options for the use-case demonstration N0 = 20 # original number of shooting nodes N12 = 15 # change the number of shooting nodes for use-cases 1 and 2 condN12 = max(1, round(N12/1)) # change the number of cond_N for use-cases 1 and 2 (for PARTIAL_* solvers only) Tf_01 = 1.0 # original final time and for use-case 1 Tf_2 = Tf_01 * 0.7 # change final time for use-case 2 (but keep N identical) # set dimensions ocp.dims.N = N0 # set cost Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2]) R = 2 * np.diag([1e-2]) ocp.cost.W_e = Q ocp.cost.W = scipy.linalg.block_diag(Q, R) ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) Vu = np.zeros((ny, nu)) Vu[4, 0] = 1.0 ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) ocp.cost.yref = np.zeros((ny,)) ocp.cost.yref_e = np.zeros((ny_e,)) # set constraints Fmax = 80 ocp.constraints.lbu = np.array([-Fmax]) ocp.constraints.ubu = np.array([+Fmax]) ocp.constraints.idxbu = np.array([0]) ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) # set options ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM, # PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' # ocp.solver_options.print_level = 1 ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP # set prediction horizon ocp.solver_options.tf = Tf_01 print(80*'-') print('generate code and compile...') if interface_type == 'cython': AcadosOcpSolver.generate(ocp, json_file='acados_ocp.json') AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) ocp_solver = AcadosOcpSolver.create_cython_solver('acados_ocp.json') elif interface_type == 'ctypes': ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json') elif interface_type == 'cython_prebuilt': from c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython ocp_solver = AcadosOcpSolverCython(ocp.model.name, ocp.solver_options.nlp_solver_type, ocp.dims.N) # test setting HPIPM options ocp_solver.options_set('qp_tol_ineq', 1e-8) ocp_solver.options_set('qp_tau_min', 1e-10) ocp_solver.options_set('qp_mu0', 1e0) # -------------------------------------------------------------------------------- # 0) solve the problem defined here (original from code export), analog to 'minimal_example_ocp.py' nvariant = 0 simX0 = np.ndarray((N0 + 1, nx)) simU0 = np.ndarray((N0, nu)) print(80*'-') print(f'solve original code with N = {N0} and Tf = {Tf_01} s:') status = ocp_solver.solve() if status != 0: ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") raise Exception(f'acados returned status {status}.') # get solution for i in range(N0): simX0[i, :] = ocp_solver.get(i, "x") simU0[i, :] = ocp_solver.get(i, "u") simX0[N0, :] = ocp_solver.get(N0, "x") ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") ocp_solver.store_iterate(filename=f'final_iterate_{interface_type}_variant{nvariant}.json', overwrite=True) if PLOT:# plot but don't halt plot_pendulum(np.linspace(0, Tf_01, N0 + 1), Fmax, simU0, simX0, latexify=False, plt_show=False, X_true_label=f'original: N={N0}, Tf={Tf_01}')
simX[0,:] = xcurrent # closed loop for i in range(Nsim): # solve ocp acados_ocp_solver.set(0, "lbx", xcurrent) acados_ocp_solver.set(0, "ubx", xcurrent) status = acados_ocp_solver.solve() if status != 0: raise Exception('acados acados_ocp_solver returned status {}. Exiting.'.format(status)) simU[i,:] = acados_ocp_solver.get(0, "u") # simulate system acados_integrator.set("x", xcurrent) acados_integrator.set("u", simU[i,:]) status = acados_integrator.solve() if status != 0: raise Exception('acados integrator returned status {}. Exiting.'.format(status)) # update state xcurrent = acados_integrator.get("x") simX[i+1,:] = xcurrent # plot results plot_pendulum(np.linspace(0, Tf/N*Nsim, Nsim+1), Fmax, simU, simX)
sim.solver_options.T = Tf # set options sim.solver_options.num_stages = 4 sim.solver_options.num_steps = 3 sim.solver_options.newton_iter = 3 # for implicit integrator # create acados_integrator = AcadosSimSolver(sim) simX = np.ndarray((N+1, nx)) x0 = np.array([0.0, np.pi+1, 0.0, 0.0]) u0 = np.array([0.0]) acados_integrator.set("u", u0) simX[0,:] = x0 for i in range(N): # set initial state acados_integrator.set("x", simX[i,:]) # solve status = acados_integrator.solve() # get solution simX[i+1,:] = acados_integrator.get("x") if status != 0: raise Exception('acados returned status {}. Exiting.'.format(status)) # plot results plot_pendulum(Tf/N, 10, np.zeros((N, nu)), simX, latexify=False)
def main(discretization='shooting_nodes'): # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = export_pendulum_ode_model() ocp.model = model integrator_type = 'LIFTED_IRK' # ERK, IRK, GNSF, LIFTED_IRK if integrator_type == 'GNSF': acados_dae_model_json_dump(model) # structure detection in Matlab/Octave -> produces 'pendulum_ode_gnsf_functions.json' status = os.system('octave detect_gnsf_from_json.m') # load gnsf from json with open(model.name + '_gnsf_functions.json', 'r') as f: gnsf_dict = json.load(f) ocp.gnsf_model = gnsf_dict Tf = 1.0 nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx N = 15 # discretization ocp.dims.N = N # shooting_nodes = np.linspace(0, Tf, N+1) time_steps = np.linspace(0, 1, N) time_steps = Tf * time_steps / sum(time_steps) shooting_nodes = np.zeros((N + 1, )) for i in range(len(time_steps)): shooting_nodes[i + 1] = shooting_nodes[i] + time_steps[i] # nonuniform discretizations can be defined either by shooting_nodes or time_steps: if discretization == 'shooting_nodes': ocp.solver_options.shooting_nodes = shooting_nodes elif discretization == 'time_steps': ocp.solver_options.time_steps = time_steps else: raise NotImplementedError( f"discretization type {discretization} not supported.") # set num_steps ocp.solver_options.sim_method_num_steps = 2 * np.ones((N, )) ocp.solver_options.sim_method_num_steps[0] = 3 # set num_stages ocp.solver_options.sim_method_num_stages = 2 * np.ones((N, )) ocp.solver_options.sim_method_num_stages[0] = 4 # set cost Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2]) R = 2 * np.diag([1e-2]) ocp.cost.W_e = Q ocp.cost.W = scipy.linalg.block_diag(Q, R) ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) Vu = np.zeros((ny, nu)) Vu[4, 0] = 1.0 ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) ocp.cost.yref = np.zeros((ny, )) ocp.cost.yref_e = np.zeros((ny_e, )) # set constraints Fmax = 80 ocp.constraints.lbu = np.array([-Fmax]) ocp.constraints.ubu = np.array([+Fmax]) x0 = np.array([0.0, np.pi, 0.0, 0.0]) ocp.constraints.x0 = x0 ocp.constraints.idxbu = np.array([0]) ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = integrator_type ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP # set prediction horizon ocp.solver_options.tf = Tf ocp.solver_options.initialize_t_slacks = 1 # Set additional options for Simulink interface: acados_path = get_acados_path() json_path = os.path.join(acados_path, 'interfaces/acados_template/acados_template') with open(json_path + '/simulink_default_opts.json', 'r') as f: simulink_opts = json.load(f) ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json', simulink_opts=simulink_opts) # ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') simX = np.ndarray((N + 1, nx)) simU = np.ndarray((N, nu)) # change options after creating ocp_solver ocp_solver.options_set("step_length", 0.99999) ocp_solver.options_set("globalization", "fixed_step") # fixed_step, merit_backtracking ocp_solver.options_set("tol_eq", TOL) ocp_solver.options_set("tol_stat", TOL) ocp_solver.options_set("tol_ineq", TOL) ocp_solver.options_set("tol_comp", TOL) # initialize solver for i in range(N): ocp_solver.set(i, "x", x0) status = ocp_solver.solve() if status not in [0, 2]: raise Exception('acados returned status {}. Exiting.'.format(status)) # get primal solution for i in range(N): simX[i, :] = ocp_solver.get(i, "x") simU[i, :] = ocp_solver.get(i, "u") simX[N, :] = ocp_solver.get(N, "x") print("inequality multipliers at stage 1") print(ocp_solver.get(1, "lam")) # inequality multipliers at stage 1 print("slack values at stage 1") print(ocp_solver.get(1, "t")) # slack values at stage 1 print("multipliers of dynamic conditions between stage 1 and 2") print(ocp_solver.get( 1, "pi")) # multipliers of dynamic conditions between stage 1 and 2 # initialize ineq multipliers and slacks at stage 1 ocp_solver.set(1, "lam", np.zeros(2, )) ocp_solver.set(1, "t", np.zeros(2, )) ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") # timings time_tot = ocp_solver.get_stats("time_tot") time_lin = ocp_solver.get_stats("time_lin") time_sim = ocp_solver.get_stats("time_sim") time_qp = ocp_solver.get_stats("time_qp") print( f"timings OCP solver: total: {1e3*time_tot}ms, lin: {1e3*time_lin}ms, sim: {1e3*time_sim}ms, qp: {1e3*time_qp}ms" ) # print("simU", simU) # print("simX", simX) iterate_filename = f'final_iterate_{discretization}.json' ocp_solver.store_iterate(filename=iterate_filename, overwrite=True) plot_pendulum(shooting_nodes, Fmax, simU, simX, latexify=False) del ocp_solver
simX[N, :] = acados_solver_ocp.get(N, "x") simY[N, :] = simX[N, :] + np.transpose( np.diag(v_stds) @ np.random.standard_normal((nx, 1))) # set measurements and controls for j in range(N): yref = np.zeros((3 * nx, )) yref[:nx] = simY[j, :] yref[2 * nx:] = x0_bar acados_solver_mhe.set(j, "yref", yref) acados_solver_mhe.set(j, "p", simU[j, :]) # solve mhe problem status = acados_solver_mhe.solve() if status != 0: raise Exception('acados returned status {}. Exiting.'.format(status)) # get solution for i in range(N): simXest[i, :] = acados_solver_mhe.get(i, "x") simWest[i, :] = acados_solver_mhe.get(i, "u") simXest[N, :] = acados_solver_mhe.get(N, "x") print('difference |x0_est - x0_bar|', np.linalg.norm(x0_bar - simXest[0, :])) print('difference |x_est - x_true|', np.linalg.norm(simXest - simX)) plot_pendulum(h, Fmax, simU, simX, simXest, simY)