def solve_marathos_problem_with_setting(setting): globalization = setting['globalization'] line_search_use_sufficient_descent = setting[ 'line_search_use_sufficient_descent'] globalization_use_SOC = setting['globalization_use_SOC'] # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = AcadosModel() x1 = SX.sym('x1') x2 = SX.sym('x2') x = vertcat(x1, x2) # dynamics: identity model.disc_dyn_expr = x model.x = x model.u = SX.sym('u', 0, 0) # [] / None doesnt work model.p = [] model.name = f'marathos_problem' ocp.model = model # discretization Tf = 1 N = 1 ocp.dims.N = N ocp.solver_options.tf = Tf # cost ocp.cost.cost_type_e = 'EXTERNAL' ocp.model.cost_expr_ext_cost_e = x1 # constarints ocp.model.con_h_expr = x1**2 + x2**2 ocp.constraints.lh = np.array([1.0]) ocp.constraints.uh = np.array([1.0]) # # soften # ocp.constraints.idxsh = np.array([0]) # ocp.cost.zl = 1e5 * np.array([1]) # ocp.cost.zu = 1e5 * np.array([1]) # ocp.cost.Zl = 1e5 * np.array([1]) # ocp.cost.Zu = 1e5 * np.array([1]) # add bounds on x # nx = 2 # ocp.constraints.idxbx_0 = np.array(range(nx)) # ocp.constraints.lbx_0 = -2 * np.ones((nx)) # ocp.constraints.ubx_0 = 2 * np.ones((nx)) # 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 = 'EXACT' ocp.solver_options.integrator_type = 'DISCRETE' # ocp.solver_options.print_level = 1 ocp.solver_options.tol = TOL ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP ocp.solver_options.globalization = globalization ocp.solver_options.alpha_min = 1e-2 # ocp.solver_options.__initialize_t_slacks = 0 # ocp.solver_options.regularize_method = 'CONVEXIFY' ocp.solver_options.levenberg_marquardt = 1e-1 # ocp.solver_options.print_level = 2 SQP_max_iter = 300 ocp.solver_options.qp_solver_iter_max = 400 ocp.solver_options.regularize_method = 'MIRROR' # ocp.solver_options.exact_hess_constr = 0 ocp.solver_options.line_search_use_sufficient_descent = line_search_use_sufficient_descent ocp.solver_options.globalization_use_SOC = globalization_use_SOC ocp.solver_options.eps_sufficient_descent = 1e-1 ocp.solver_options.qp_tol = 5e-7 if FOR_LOOPING: # call solver in for loop to get all iterates ocp.solver_options.nlp_solver_max_iter = 1 ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') else: ocp.solver_options.nlp_solver_max_iter = SQP_max_iter ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') # initialize solver rad_init = 0.1 #0.1 #np.pi / 4 xinit = np.array([np.cos(rad_init), np.sin(rad_init)]) # xinit = np.array([0.82120912, 0.58406911]) [ocp_solver.set(i, "x", xinit) for i in range(N + 1)] # solve if FOR_LOOPING: # call solver in for loop to get all iterates iterates = np.zeros((SQP_max_iter + 1, 2)) iterates[0, :] = xinit alphas = np.zeros((SQP_max_iter, )) qp_iters = np.zeros((SQP_max_iter, )) iter = SQP_max_iter residuals = np.zeros((4, SQP_max_iter)) # solve for i in range(SQP_max_iter): status = ocp_solver.solve() ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") # print(f'acados returned status {status}.') iterates[i + 1, :] = ocp_solver.get(0, "x") if status in [0, 4]: iter = i break alphas[i] = ocp_solver.get_stats('alpha')[1] qp_iters[i] = ocp_solver.get_stats('qp_iter')[1] residuals[:, i] = ocp_solver.get_stats('residuals') else: ocp_solver.solve() ocp_solver.print_statistics() iter = ocp_solver.get_stats('sqp_iter')[0] alphas = ocp_solver.get_stats('alpha')[1:] qp_iters = ocp_solver.get_stats('qp_iter') residuals = ocp_solver.get_stats('statistics')[1:5, 1:iter] # get solution solution = ocp_solver.get(0, "x") # print summary print(f"solved Marathos test problem with settings {setting}") print( f"cost function value = {ocp_solver.get_cost()} after {iter} SQP iterations" ) print(f"alphas: {alphas[:iter]}") print(f"total number of QP iterations: {sum(qp_iters[:iter])}") max_infeasibility = np.max(residuals[1:3]) print(f"max infeasibility: {max_infeasibility}") # compare to analytical solution exact_solution = np.array([-1, 0]) sol_err = max(np.abs(solution - exact_solution)) # checks if sol_err > TOL * 1e1: raise Exception( f"error of numerical solution wrt exact solution = {sol_err} > tol = {TOL*1e1}" ) else: print(f"matched analytical solution with tolerance {TOL}") try: if globalization == 'FIXED_STEP': # import pdb; pdb.set_trace() if max_infeasibility < 5.0: raise Exception( f"Expected max_infeasibility > 5.0 when using full step SQP on Marathos problem" ) if iter != 10: raise Exception( f"Expected 10 SQP iterations when using full step SQP on Marathos problem, got {iter}" ) if any(alphas[:iter] != 1.0): raise Exception( f"Expected all alphas = 1.0 when using full step SQP on Marathos problem" ) elif globalization == 'MERIT_BACKTRACKING': if max_infeasibility > 0.5: raise Exception( f"Expected max_infeasibility < 0.5 when using globalized SQP on Marathos problem" ) if globalization_use_SOC == 0: if FOR_LOOPING and iter != 57: raise Exception( f"Expected 57 SQP iterations when using globalized SQP without SOC on Marathos problem, got {iter}" ) elif line_search_use_sufficient_descent == 1: if iter not in range(29, 37): # NOTE: got 29 locally and 36 on Github actions. # On Github actions the inequality constraint was numerically violated in the beginning. # This leads to very different behavior, since the merit gradient is so different. # Github actions: merit_grad = -1.669330e+00, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = -1.495535e+00 # Jonathan Laptop: merit_grad = -1.737950e-01, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = 0.000000e+00 raise Exception( f"Expected SQP iterations in range(29, 37) when using globalized SQP with SOC on Marathos problem, got {iter}" ) else: if iter != 12: raise Exception( f"Expected 12 SQP iterations when using globalized SQP with SOC on Marathos problem, got {iter}" ) except Exception as inst: if FOR_LOOPING and globalization == "MERIT_BACKTRACKING": print( "\nAcados globalized OCP solver behaves different when for looping due to different merit function weights.", "Following exception is not raised\n") print(inst, "\n") else: raise (inst) if PLOT: plt.figure() axs = plt.plot(solution[0], solution[1], 'x', label='solution') if FOR_LOOPING: # call solver in for loop to get all iterates cm = plt.cm.get_cmap('RdYlBu') axs = plt.scatter(iterates[:iter + 1, 0], iterates[:iter + 1, 1], c=range(iter + 1), s=35, cmap=cm, label='iterates') plt.colorbar(axs) ts = np.linspace(0, 2 * np.pi, 100) plt.plot(1 * np.cos(ts) + 0, 1 * np.sin(ts) - 0, 'r') plt.axis('square') plt.legend() plt.title( f"Marathos problem with N = {N}, x formulation, SOC {globalization_use_SOC}" ) plt.show() print(f"\n\n----------------------\n")
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)
def solve_marathos_ocp(setting): globalization = setting['globalization'] line_search_use_sufficient_descent = setting[ 'line_search_use_sufficient_descent'] globalization_use_SOC = setting['globalization_use_SOC'] qp_solver = setting['qp_solver'] # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = export_linear_mass_model() ocp.model = model nx = model.x.size()[0] nu = model.u.size()[0] ny = nu # discretization Tf = 2 N = 20 shooting_nodes = np.linspace(0, Tf, N + 1) ocp.dims.N = N # set cost Q = 2 * np.diag([]) R = 2 * np.diag([1e1, 1e1]) 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)) Vu = np.eye((nu)) ocp.cost.Vu = Vu ocp.cost.yref = np.zeros((ny, )) # set constraints Fmax = 5 ocp.constraints.lbu = -Fmax * np.ones((nu, )) ocp.constraints.ubu = +Fmax * np.ones((nu, )) ocp.constraints.idxbu = np.array(range(nu)) x0 = np.array([1e-1, 1.1, 0, 0]) ocp.constraints.x0 = x0 # terminal constraint x_goal = np.array([0, -1.1, 0, 0]) ocp.constraints.idxbx_e = np.array(range(nx)) ocp.constraints.lbx_e = x_goal ocp.constraints.ubx_e = x_goal if SOFTEN_TERMINAL: ocp.constraints.idxsbx_e = np.array(range(nx)) ocp.cost.zl_e = 1e4 * np.ones(nx) ocp.cost.zu_e = 1e4 * np.ones(nx) ocp.cost.Zl_e = 1e6 * np.ones(nx) ocp.cost.Zu_e = 1e6 * np.ones(nx) # add obstacle if OBSTACLE: obs_rad = 1.0 obs_x = 0.0 obs_y = 0.0 circle = (obs_x, obs_y, obs_rad) ocp.constraints.uh = np.array([100.0]) # doenst matter ocp.constraints.lh = np.array([obs_rad**2]) x_square = model.x[0]**OBSTACLE_POWER + model.x[1]**OBSTACLE_POWER ocp.model.con_h_expr = x_square # copy for terminal ocp.constraints.uh_e = ocp.constraints.uh ocp.constraints.lh_e = ocp.constraints.lh ocp.model.con_h_expr_e = ocp.model.con_h_expr else: circle = None # soften if OBSTACLE and SOFTEN_OBSTACLE: ocp.constraints.idxsh = np.array([0]) ocp.constraints.idxsh_e = np.array([0]) Zh = 1e6 * np.ones(1) zh = 1e4 * np.ones(1) ocp.cost.zl = zh ocp.cost.zu = zh ocp.cost.Zl = Zh ocp.cost.Zu = Zh ocp.cost.zl_e = np.concatenate((ocp.cost.zl_e, zh)) ocp.cost.zu_e = np.concatenate((ocp.cost.zu_e, zh)) ocp.cost.Zl_e = np.concatenate((ocp.cost.Zl_e, Zh)) ocp.cost.Zu_e = np.concatenate((ocp.cost.Zu_e, Zh)) # set options ocp.solver_options.qp_solver = qp_solver # 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 ocp.solver_options.globalization = globalization ocp.solver_options.alpha_min = 0.01 # ocp.solver_options.__initialize_t_slacks = 0 # ocp.solver_options.levenberg_marquardt = 1e-2 ocp.solver_options.qp_solver_cond_N = 0 ocp.solver_options.print_level = 1 ocp.solver_options.nlp_solver_max_iter = 200 ocp.solver_options.qp_solver_iter_max = 400 # NOTE: this is needed for PARTIAL_CONDENSING_HPIPM to get expected behavior qp_tol = 5e-7 ocp.solver_options.qp_solver_tol_stat = qp_tol ocp.solver_options.qp_solver_tol_eq = qp_tol ocp.solver_options.qp_solver_tol_ineq = qp_tol ocp.solver_options.qp_solver_tol_comp = qp_tol ocp.solver_options.qp_solver_ric_alg = 1 # ocp.solver_options.qp_solver_cond_ric_alg = 1 # set prediction horizon ocp.solver_options.tf = Tf ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}_ocp.json') ocp_solver.options_set('line_search_use_sufficient_descent', line_search_use_sufficient_descent) ocp_solver.options_set('globalization_use_SOC', globalization_use_SOC) ocp_solver.options_set('full_step_dual', 1) if INITIALIZE: # initialize solver # [ocp_solver.set(i, "x", x0 + (i/N) * (x_goal-x0)) for i in range(N+1)] [ocp_solver.set(i, "x", x0) for i in range(N + 1)] # [ocp_solver.set(i, "u", 2*(np.random.rand(2) - 0.5)) for i in range(N)] # solve status = ocp_solver.solve() ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") sqp_iter = ocp_solver.get_stats('sqp_iter')[0] print(f'acados returned status {status}.') # ocp_solver.store_iterate(f'it{ocp.solver_options.nlp_solver_max_iter}_{model.name}.json') # get solution simX = np.array([ocp_solver.get(i, "x") for i in range(N + 1)]) simU = np.array([ocp_solver.get(i, "u") for i in range(N)]) pi_multiplier = [ocp_solver.get(i, "pi") for i in range(N)] print(f"cost function value = {ocp_solver.get_cost()}") # print summary print(f"solved Marathos test problem with settings {setting}") print( f"cost function value = {ocp_solver.get_cost()} after {sqp_iter} SQP iterations" ) # print(f"alphas: {alphas[:iter]}") # print(f"total number of QP iterations: {sum(qp_iters[:iter])}") # max_infeasibility = np.max(residuals[1:3]) # print(f"max infeasibility: {max_infeasibility}") # checks if status != 0: raise Exception(f"acados solver returned status {status} != 0.") if globalization == "FIXED_STEP": if sqp_iter != 18: raise Exception( f"acados solver took {sqp_iter} iterations, expected 18.") elif globalization == "MERIT_BACKTRACKING": if globalization_use_SOC == 1 and line_search_use_sufficient_descent == 0 and sqp_iter not in range( 21, 23): raise Exception( f"acados solver took {sqp_iter} iterations, expected range(21, 23)." ) elif globalization_use_SOC == 1 and line_search_use_sufficient_descent == 1 and sqp_iter not in range( 21, 24): raise Exception( f"acados solver took {sqp_iter} iterations, expected range(21, 24)." ) elif globalization_use_SOC == 0 and line_search_use_sufficient_descent == 0 and sqp_iter not in range( 155, 165): raise Exception( f"acados solver took {sqp_iter} iterations, expected range(155, 165)." ) elif globalization_use_SOC == 0 and line_search_use_sufficient_descent == 1 and sqp_iter not in range( 160, 175): raise Exception( f"acados solver took {sqp_iter} iterations, expected range(160, 175)." ) if PLOT: plot_linear_mass_system_X_state_space(simX, circle=circle, x_goal=x_goal) plot_linear_mass_system_U(shooting_nodes, simU) # plot_linear_mass_system_X(shooting_nodes, simX) # import pdb; pdb.set_trace() print(f"\n\n----------------------\n")
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
def solve_armijo_problem_with_setting(setting): globalization = setting['globalization'] line_search_use_sufficient_descent = setting[ 'line_search_use_sufficient_descent'] globalization_use_SOC = setting['globalization_use_SOC'] # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = AcadosModel() x = SX.sym('x') # dynamics: identity model.disc_dyn_expr = x model.x = x model.u = SX.sym('u', 0, 0) # [] / None doesnt work model.p = [] model.name = f'armijo_problem' ocp.model = model # discretization Tf = 1 N = 1 ocp.dims.N = N ocp.solver_options.tf = Tf # cost ocp.cost.cost_type_e = 'EXTERNAL' ocp.model.cost_expr_ext_cost_e = x @ x ocp.model.cost_expr_ext_cost_custom_hess_e = 1.0 # 2.0 is the actual hessian # constarints ocp.constraints.idxbx = np.array([0]) ocp.constraints.lbx = np.array([-10.0]) ocp.constraints.ubx = np.array([10.0]) # options ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' # 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = 'EXACT' ocp.solver_options.integrator_type = 'DISCRETE' ocp.solver_options.print_level = 0 ocp.solver_options.tol = TOL ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP ocp.solver_options.globalization = globalization ocp.solver_options.alpha_reduction = 0.9 ocp.solver_options.line_search_use_sufficient_descent = line_search_use_sufficient_descent ocp.solver_options.globalization_use_SOC = globalization_use_SOC ocp.solver_options.eps_sufficient_descent = 5e-1 SQP_max_iter = 200 ocp.solver_options.qp_solver_iter_max = 400 ocp.solver_options.nlp_solver_max_iter = SQP_max_iter # create solver ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') # initialize solver xinit = np.array([1.0]) [ocp_solver.set(i, "x", xinit) for i in range(N + 1)] # get stats status = ocp_solver.solve() ocp_solver.print_statistics() iter = ocp_solver.get_stats('sqp_iter')[0] alphas = ocp_solver.get_stats('alpha')[1:] qp_iters = ocp_solver.get_stats('qp_iter') print(f"acados ocp solver returned status {status}") # get solution solution = ocp_solver.get(0, "x") print(f"found solution {solution}") # print summary print(f"solved Armijo test problem with settings {setting}") print( f"cost function value = {ocp_solver.get_cost()} after {iter} SQP iterations" ) print(f"alphas: {alphas[:iter]}") print(f"total number of QP iterations: {sum(qp_iters[:iter])}") # compare to analytical solution exact_solution = np.array([0.0]) sol_err = max(np.abs(solution - exact_solution)) print(f"error wrt analytical solution {sol_err}") # checks if ocp.model.cost_expr_ext_cost_custom_hess_e == 1.0: if globalization == 'MERIT_BACKTRACKING': if sol_err > TOL * 1e1: raise Exception( f"error of numerical solution wrt exact solution = {sol_err} > tol = {TOL*1e1}" ) else: print(f"matched analytical solution with tolerance {TOL}") if status != 0: raise Exception( f"acados solver returned status {status} != 0.") if line_search_use_sufficient_descent == 1: if iter > 22: raise Exception(f"acados ocp solver took {iter} iterations." + \ "Expected <= 22 iterations for globalized SQP method with aggressive eps_sufficient_descent condition on Armijo test problem.") else: if iter < 64: raise Exception(f"acados ocp solver took {iter} iterations." + \ "Expected > 64 iterations for globalized SQP method without sufficient descent condition on Armijo test problem.") elif globalization == 'FIXED_STEP': if status != 2: raise Exception( f"acados solver returned status {status} != 2. Expected maximum iterations for full-step SQP on Armijo test problem." ) else: print( f"Sucess: Expected maximum iterations for full-step SQP on Armijo test problem." ) print(f"\n\n----------------------\n")
class Pmpc(object): def __init__(self, N, sys, cost, wref=None, tuning=None, lam_g_ref=None, sensitivities=None, options={}): """ Constructor """ # store construction data self.__N = N self.__vars = sys['vars'] self.__nx = sys['vars']['x'].shape[0] self.__nu = sys['vars']['u'].shape[0] # nonlinear inequalities slacks if 'us' in sys['vars']: self.__ns = sys['vars']['us'].shape[0] else: self.__ns = 0 # mpc slacks if 'usc' in sys['vars']: self.__nsc = sys['vars']['usc'].shape[0] self.__scost = sys['scost'] else: self.__nsc = 0 # store system dynamics self.__F = sys['f'] # store path constraints if 'h' in sys: self.__h = sys['h'] h_lin = self.__h(*self.__vars.values()) self.__h_x_idx = [ idx for idx in range(h_lin.shape[0]) if not True in ca.which_depends( h_lin[idx], ct.vertcat(*list(self.__vars.values())[1:])) ] else: self.__h = None # store slacked nonlinear inequality constraints if 'g' in sys: self.__gnl = sys['g'] self.__detect_state_dependent_constraints() else: self.__gnl = None self.__h_us_idx = [] # no nonlinear state-dependent constraints # store system sensitivities around steady state self.__S = sys['S'] self.__cost = cost # set options self.__options = self.__default_options() for option in options: if option in self.__options: self.__options[option] = options[option] else: raise ValueError( 'Unknown option for Pmpc class instance: "{}"'.format( option)) # detect cost-type if self.__cost.n_in() == 2: # cost function of the form: l(x,u) self.__type = 'economic' # no tuning required tuning = None if self.__options['hessian_approximation'] == 'gauss_newton': self.__options['hessian_approximation'] = 'exact' Logger.logger.warning( 'Gauss-Newton Hessian approximation cannot be applied for economic MPC problem. Switched to exact Hessian.' ) else: # cost function of the form: (w-wref)'*H*(w-wref) + q'w self.__type = 'tracking' # check if tuning matrices are provided assert tuning != None, 'Provide tuning matrices for tracking MPC!' # periodicity operator self.__p_operator = self.__options['p_operator'] self.__jac_p_operator = ca.Function('jac_p', [sys['vars']['x']], [ ca.jacobian(self.__p_operator(sys['vars']['x']), sys['vars']['x']) ]) self.__S = sensitivities # construct MPC solver self.__construct_solver() # periodic indexing self.__index = 0 self.__index_acados = 0 # create periodic reference assert wref != None, 'Provide reference trajectory!' self.__create_reference(wref, tuning, lam_g_ref) # initialize log self.__initialize_log() # initialize acados solvers self.__acados_ocp_solver = None self.__acados_integrator = None # solver initial guess self.__set_initial_guess() return None def __default_options(self): # default options opts = { 'hessian_approximation': 'exact', 'ipopt_presolve': False, 'max_iter': 2000, 'p_operator': ca.Function('p_operator', [self.__vars['x']], [self.__vars['x']]), 'slack_flag': 'none' } return opts def __construct_solver(self): """ Construct periodic MPC solver """ # system variables and dimensions x = self.__vars['x'] u = self.__vars['u'] # NLP parameters if self.__type == 'economic': # parameters self.__p = ct.struct_symMX([ ct.entry('x0', shape=(self.__nx, 1)), ct.entry('xN', shape=(self.__nx, 1)) ]) # reassign for brevity x0 = self.__p['x0'] xN = self.__p['xN'] if self.__type == 'tracking': ref_vars = (ct.entry('x', shape=(self.__nx, ), repeat=self.__N + 1), ct.entry('u', shape=(self.__nu, ), repeat=self.__N)) if 'us' in self.__vars: ref_vars += (ct.entry('us', shape=(self.__ns, ), repeat=self.__N), ) # reference trajectory wref = ct.struct_symMX([ref_vars]) nw = self.__nx + self.__nu + self.__ns tuning = ct.struct_symMX([ # tracking tuning ct.entry('H', shape=(nw, nw), repeat=self.__N), ct.entry('q', shape=(nw, 1), repeat=self.__N) ]) # parameters self.__p = ct.struct_symMX([ ct.entry('x0', shape=(self.__nx, )), ct.entry('wref', struct=wref), ct.entry('tuning', struct=tuning) ]) # reassign for brevity x0 = self.__p['x0'] wref = self.__p.prefix['wref'] tuning = self.__p.prefix['tuning'] xN = wref['x', -1] # NLP variables variables_entry = (ct.entry('x', shape=(self.__nx, ), repeat=self.__N + 1), ct.entry('u', shape=(self.__nu, ), repeat=self.__N)) if 'us' in self.__vars: variables_entry += (ct.entry('us', shape=(self.__ns, ), repeat=self.__N), ) self.__wref = ct.struct_symMX([variables_entry ]) # structure of reference if 'usc' in self.__vars: variables_entry += (ct.entry('usc', shape=(self.__nsc, ), repeat=self.__N), ) # nlp variables + bounds w = ct.struct_symMX([variables_entry]) # variable bounds are implemented as inequalities self.__lbw = w(-np.inf) self.__ubw = w(np.inf) # prepare dynamics and path constraints entry constraints_entry = (ct.entry('dyn', shape=(self.__nx, ), repeat=self.__N), ) if self.__gnl is not None: constraints_entry += (ct.entry('g', shape=self.__gnl.size1_out(0), repeat=self.__N), ) if self.__h is not None: constraints_entry += (ct.entry('h', shape=self.__h.size1_out(0), repeat=self.__N), ) # terminal constraint self.__nx_term = self.__p_operator.size1_out(0) # create general constraints structure g_struct = ct.struct_symMX([ ct.entry('init', shape=(self.__nx, 1)), constraints_entry, ct.entry('term', shape=(self.__nx_term, 1)) ]) # create symbolic constraint expressions map_args = collections.OrderedDict() map_args['x0'] = ct.horzcat(*w['x', :-1]) map_args['p'] = ct.horzcat(*w['u']) F_constr = ct.horzsplit(self.__F.map(self.__N)(**map_args)['xf']) # generate constraints constr = collections.OrderedDict() constr['dyn'] = [a - b for a, b in zip(F_constr, w['x', 1:])] if 'us' in self.__vars: map_args['us'] = ct.horzcat(*w['us']) if self.__gnl is not None: constr['g'] = ct.horzsplit( self.__gnl.map(self.__N)(*map_args.values())) if 'usc' in self.__vars: map_args['usc'] = ct.horzcat(*w['usc']) if self.__h is not None: constr['h'] = ct.horzsplit( self.__h.map(self.__N)(*map_args.values())) repeated_constr = list( itertools.chain.from_iterable(zip(*constr.values()))) term_constraint = self.__p_operator(w['x', -1] - xN) self.__g = g_struct( ca.vertcat(w['x', 0] - x0, *repeated_constr, term_constraint)) self.__lbg = g_struct(np.zeros(self.__g.shape)) self.__ubg = g_struct(np.zeros(self.__g.shape)) if self.__h is not None: self.__ubg['h', :] = np.inf for i in self.__h_us_idx + self.__h_x_idx: # rm constraints the only depend on x at k = 0 self.__lbg['h', 0, i] = -np.inf # nlp cost cost_map = self.__cost.map(self.__N) if self.__type == 'economic': cost_args = [ct.horzcat(*w['x', :-1]), ct.horzcat(*w['u'])] elif self.__type == 'tracking': if self.__ns != 0: cost_args_w = ct.horzcat(*[ ct.vertcat(w['x', k], w['u', k], w['us', k]) for k in range(self.__N) ]) cost_args_w_ref = ct.horzcat(*[ ct.vertcat(wref['x', k], wref['u', k], wref['us', k]) for k in range(self.__N) ]) else: cost_args_w = ct.horzcat(*[ ct.vertcat(w['x', k], w['u', k]) for k in range(self.__N) ]) cost_args_w_ref = ct.horzcat(*[ ct.vertcat(wref['x', k], wref['u', k]) for k in range(self.__N) ]) cost_args = [ cost_args_w, cost_args_w_ref, ct.horzcat(*tuning['H']), ct.horzcat(*tuning['q']) ] if self.__options['hessian_approximation'] == 'gauss_newton': if 'usc' not in self.__vars: hess_gn = ct.diagcat(*tuning['H'], ca.DM.zeros(self.__nx, self.__nx)) else: hess_block = list( itertools.chain.from_iterable( zip(tuning['H'], [ca.DM.zeros(self.__nsc, self.__nsc)] * self.__N))) hess_gn = ct.diagcat(*hess_block, ca.DM.zeros(self.__nx, self.__nx)) J = ca.sum2(cost_map(*cost_args)) # add cost on slacks if 'usc' in self.__vars: J += ca.sum2(ct.mtimes(self.__scost.T, ct.horzcat(*w['usc']))) # create solver prob = {'f': J, 'g': self.__g, 'x': w, 'p': self.__p} self.__w = w self.__g_fun = ca.Function('g_fun', [self.__w, self.__p], [self.__g]) # create IPOPT-solver instance if needed if self.__options['ipopt_presolve']: opts = { 'ipopt': { 'linear_solver': 'ma57', 'print_level': 0 }, 'expand': False } if Logger.logger.getEffectiveLevel() > 10: opts['ipopt']['print_level'] = 0 opts['print_time'] = 0 opts['ipopt']['sb'] = 'yes' self.__solver = ca.nlpsol('solver', 'ipopt', prob, opts) # create hessian approximation function if self.__options['hessian_approximation'] == 'gauss_newton': lam_g = ca.MX.sym('lam_g', self.__g.shape) # will not be used hess_approx = ca.Function('hess_approx', [self.__w, self.__p, lam_g], [hess_gn]) elif self.__options['hessian_approximation'] == 'exact': hess_approx = 'exact' # create sqp solver prob['lbg'] = self.__lbg prob['ubg'] = self.__ubg sqp_opts = { 'hessian_approximation': hess_approx, 'max_iter': self.__options['max_iter'] } self.__sqp_solver = sqp_method.Sqp(prob, sqp_opts) def step(self, x0): """ Compute periodic MPC feedback control for given initial condition. """ # reset periodic indexing if necessary self.__index = self.__index % len(self.__ref) # update nlp parameters p0 = self.__p(0.0) p0['x0'] = x0 if self.__type == 'economic': p0['xN'] = self.__ref[self.__index][-x0.shape[0]:] elif self.__type == 'tracking': p0['wref'] = self.__ref[self.__index] p0['tuning', 'H'] = self.__Href[self.__index] p0['tuning', 'q'] = self.__qref[self.__index] # pre-solve NLP with IPOPT for globalization if self.__options['ipopt_presolve']: ipopt_sol = self.__solver(x0=self.__w0, lbg=self.__lbg, ubg=self.__ubg, p=p0) self.__w0 = self.__w(ipopt_sol['x']) self.__lam_g0 = self.__g(ipopt_sol['lam_g']) # solve NLP sol = self.__sqp_solver.solve(self.__w0.cat, p0.cat, self.__lam_g0.cat) # store solution self.__g_sol = self.__g(self.__g_fun(sol['x'], p0)) self.__w_sol = self.__w(sol['x']) self.__extract_solver_stats() # shift reference self.__index += 1 # update initial guess self.__w0, self.__lam_g0 = self.__shift_initial_guess( self.__w_sol, self.__g(sol['lam_g'])) return self.__w_sol['u', 0] def step_acados(self, x0): # reset periodic indexing if necessary self.__index_acados = self.__index_acados % self.__Nref # format x0 x0 = np.squeeze(x0.full()) # update NLP parameters self.__acados_ocp_solver.set(0, "lbx", x0) self.__acados_ocp_solver.set(0, "ubx", x0) # update reference and tuning matrices self.__set_acados_reference() # solve status = self.__acados_ocp_solver.solve() # timings # np.append(self.__acados_times, self.__acados_ocp_solver.get_stats("time_tot")) print("acados timings: total: ", self.__acados_ocp_solver.get_stats("time_tot"), \ " lin: ", self.__acados_ocp_solver.get_stats("time_lin"), \ " sim: ", self.__acados_ocp_solver.get_stats("time_sim"), " qp: ", \ self.__acados_ocp_solver.get_stats("time_qp")) # if status != 0: # raise Exception('acados solver returned status {}. Exiting.'.format(status)) # save solution self.__w_sol_acados = self.__w(0.0) for i in range(self.__N): self.__w_sol_acados['x', i] = self.__acados_ocp_solver.get(i, "x") self.__w_sol_acados['u', i] = self.__acados_ocp_solver.get( i, "u")[:self.__nu] if 'us' in self.__vars: self.__w_sol_acados['us', i] = self.__acados_ocp_solver.get( i, "u")[self.__nu:] self.__w_sol_acados['x', self.__N] = self.__acados_ocp_solver.get( self.__N, "x") self.__extract_acados_solver_stats() # feedback policy u0 = self.__acados_ocp_solver.get(0, "u")[:self.__nu] # update initial guess self.__shift_initial_guess_acados() # shift index self.__index_acados += 1 return u0 def generate(self, dae=None, quad=None, name='tunempc', opts={}): """ Create embeddable NLP solver """ from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, AcadosSimSolver # extract dimensions nx = self.__nx nu = self.__nu + self.__ns # treat slacks as pseudo-controls # extract reference ref = self.__ref xref = np.squeeze(self.__ref[0][:nx], axis=1) uref = np.squeeze(self.__ref[0][nx:nx + nu], axis=1) # sampling time self.__ts = opts['tf'] / self.__N # create acados model model = AcadosModel() model.x = ca.MX.sym('x', nx) model.u = ca.MX.sym('u', nu) model.p = [] model.name = name # detect input type if dae is None: model.f_expl_expr = self.__F(x0=model.x, p=model.u)['xf'] / self.__ts opts['integrator_type'] = 'ERK' opts['sim_method_num_stages'] = 1 opts['sim_method_num_steps'] = 1 else: n_in = dae.n_in() if n_in == 2: # xdot = f(x, u) if 'integrator_type' in opts: if opts['integrator_type'] in ['IRK', 'GNSF']: xdot = ca.MX.sym('xdot', nx) model.xdot = xdot model.f_impl_expr = xdot - dae(model.x, model.u[:self.__nu]) model.f_expl_expr = xdot elif opts['integrator_type'] == 'ERK': model.f_expl_expr = dae(model.x, model.u[:self.__nu]) else: raise ValueError('Provide numerical integrator type!') else: xdot = ca.MX.sym('xdot', nx) model.xdot = xdot model.f_expl_expr = xdot if n_in == 3: # f(xdot, x, u) = 0 model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu]) elif n_in == 4: # f(xdot, x, u, z) = 0 nz = dae.size1_in(3) z = ca.MX.sym('z', nz) model.z = z model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu], z) else: raise ValueError( 'Invalid number of inputs for system dynamics function.' ) if self.__gnl is not None: model.con_h_expr = self.__gnl(model.x, model.u[:self.__nu], model.u[self.__nu:]) if self.__type == 'economic': if quad is None: model.cost_expr_ext_cost = self.__cost( model.x, model.u[:self.__nu]) / self.__ts else: model.cost_expr_ext_cost = self.__cost(model.x, model.u[:self.__nu]) # create acados ocp ocp = AcadosOcp() ocp.model = model ny = nx + nu ny_e = nx if 'integrator_type' in opts and opts['integrator_type'] == 'GNSF': from acados_template import acados_dae_model_json_dump import os acados_dae_model_json_dump(model) # Set up Octave to be able to run the following: ## if using a virtual python env, the following lines can be added to the env/bin/activate script: # export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/external/casadi-octave # export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/ # export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/ # echo # echo "OCTAVE_PATH=$OCTAVE_PATH" status = os.system( "octave --eval \"convert({})\"".format("\'" + model.name + "_acados_dae.json\'")) # 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 horizon length ocp.dims.N = self.__N # set cost module if self.__type == 'economic': # set cost function type to external (provided in model) ocp.cost.cost_type = 'EXTERNAL' if quad is not None: ocp.solver_options.cost_discretization = 'INTEGRATOR' elif self.__type == 'tracking': # set weighting matrices ocp.cost.W = self.__Href[0][0] # set-up linear least squares cost ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.W_e = np.zeros((nx, nx)) ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) Vu = np.zeros((ny, nu)) Vu[nx:, :] = np.eye(nu) ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) ocp.cost.yref = np.squeeze( ca.vertcat(xref,uref).full() - \ ct.mtimes(np.linalg.inv(ocp.cost.W),self.__qref[0][0].T).full(), # gradient term axis = 1 ) ocp.cost.yref_e = np.zeros((ny_e, )) if n_in == 4: # DAE flag ocp.cost.Vz = np.zeros((ny, nz)) # if 'custom_hessian' in opts: # self.__custom_hessian = opts['custom_hessian'] # initial condition ocp.constraints.x0 = xref # set inequality constraints ocp.constraints.constr_type = 'BGH' if self.__S['C'] is not None: C = self.__S['C'][0][:, :nx] D = self.__S['C'][0][:, nx:] lg = -self.__S['e'][0] + ct.mtimes(C, xref).full() + ct.mtimes( D, uref).full() ug = 1e8 - self.__S['e'][0] + ct.mtimes( C, xref).full() + ct.mtimes(D, uref).full() ocp.constraints.lg = np.squeeze(lg, axis=1) ocp.constraints.ug = np.squeeze(ug, axis=1) ocp.constraints.C = C ocp.constraints.D = D if 'usc' in self.__vars: if 'us' in self.__vars: arg = [ self.__vars['x'], self.__vars['u'], self.__vars['us'], self.__vars['usc'] ] else: arg = [ self.__vars['x'], self.__vars['u'], self.__vars['usc'] ] Jsg = ca.Function( 'Jsg', [self.__vars['usc']], [ca.jacobian(self.__h(*arg), self.__vars['usc'])])(0.0) self.__Jsg = Jsg.full()[:-self.__nsc, :] ocp.constraints.Jsg = self.__Jsg ocp.cost.Zl = np.zeros((self.__nsc, )) ocp.cost.Zu = np.zeros((self.__nsc, )) ocp.cost.zl = np.squeeze(self.__scost.full(), axis=1) / self.__ts ocp.cost.zu = np.squeeze(self.__scost.full(), axis=1) / self.__ts # set nonlinear equality constraints if self.__gnl is not None: ocp.constraints.lh = np.zeros(self.__ns, ) ocp.constraints.uh = np.zeros(self.__ns, ) # terminal constraint: x_term = self.__p_operator(model.x) Jbx = ca.Function('Jbx', [model.x], [ca.jacobian(x_term, model.x)])(0.0) ocp.constraints.Jbx_e = Jbx.full() ocp.constraints.lbx_e = np.squeeze(self.__p_operator(xref).full(), axis=1) ocp.constraints.ubx_e = np.squeeze(self.__p_operator(xref).full(), axis=1) for option in list(opts.keys()): if hasattr(ocp.solver_options, option): setattr(ocp.solver_options, option, opts[option]) self.__acados_ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp_' + model.name + '.json') self.__acados_integrator = AcadosSimSolver(ocp, json_file='acados_ocp_' + model.name + '.json') # set initial guess self.__set_acados_initial_guess() return self.__acados_ocp_solver, self.__acados_integrator def __create_reference(self, wref, tuning, lam_g_ref): """ Create periodic reference and tuning data. """ # period of reference self.__Nref = len(wref['u']) # create reference and tuning sequence # for each starting point in period ref_pr = [] ref_du = [] ref_du_struct = [] H = [] q = [] for k in range(self.__Nref): # reference primal solution refk = [] for j in range(self.__N): refk += [ wref['x', (k + j) % self.__Nref], wref['u', (k + j) % self.__Nref] ] if 'us' in self.__vars: refk += [wref['us', (k + j) % self.__Nref]] refk.append(wref['x', (k + self.__N) % self.__Nref]) # reference dual solution lamgk = self.__g(0.0) lamgk['init'] = -lam_g_ref['dyn', (k - 1) % self.__Nref] for j in range(self.__N): lamgk['dyn', j] = lam_g_ref['dyn', (k + j) % self.__Nref] if 'g' in list(lamgk.keys()): lamgk['g', j] = lam_g_ref['g', (k + j) % self.__Nref] if 'h' in list(lamgk.keys()): lam_h = [lam_g_ref['h', (k + j) % self.__Nref]] if 'usc' in self.__vars: lam_h += [-self.__scost] # TODO not entirely correct lamgk['h', j] = ct.vertcat(*lam_h) lamgk['term'] = self.__p_operator( lam_g_ref['dyn', (k + self.__N - 1) % self.__Nref]) # adjust dual solution of terminal constraint is projected if self.__nx_term != self.__nx: # find new terminal multiplier A_m = [] b_m = [] A_factor = ca.DM.eye(self.__nx) for j in range(self.__N): A_m.append( ct.mtimes( ct.mtimes( self.__S['B'][(self.__N - j - 1) % self.__Nref].T, A_factor), self.__jac_p_operator(ca.DM.ones(self.__nx, 1)).T)) b_m.append( ct.mtimes( ct.mtimes( self.__S['B'][(self.__N - j - 1) % self.__Nref].T, A_factor), lam_g_ref['dyn', (k + self.__N - 1) % self.__Nref])) A_factor = ct.mtimes( self.__S['A'][(self.__N - j - 1) % self.__Nref].T, A_factor) A_m = ct.vertcat(*A_m) b_m = ct.vertcat(*b_m) LI_indeces = [ ] # indeces of first full rank number linearly independent rows R0 = 0 for i in range(A_m.shape[0]): R = np.linalg.matrix_rank(A_m[LI_indeces + [i], :]) if R > R0: LI_indeces.append(i) R0 = R lamgk['term'] = ca.solve(A_m[LI_indeces, :], b_m[LI_indeces, :]) # recursively update dynamics multipliers delta_lam = -lam_g_ref['dyn', (k + self.__N - 1) % self.__Nref] + ct.mtimes( self.__jac_p_operator( ca.DM.ones(self.__nx, 1)).T, lamgk['term']) lamgk['dyn', self.__N - 1] += delta_lam for j in range(1, self.__N + 1): delta_lam = ct.mtimes( self.__S['A'][(self.__N - j) % self.__Nref].T, delta_lam) if j < self.__N: lamgk['dyn', self.__N - 1 - j] += delta_lam else: lamgk['init'] += -delta_lam ref_pr.append(ct.vertcat(*refk)) ref_du.append(lamgk.cat) ref_du_struct.append(lamgk) if tuning is not None: H.append([ tuning['H'][(k + j) % self.__Nref] for j in range(self.__N) ]) q.append([ tuning['q'][(k + j) % self.__Nref] for j in range(self.__N) ]) self.__ref = ref_pr self.__ref_du = ref_du self.__ref_du_struct = ref_du_struct self.__Href = H self.__qref = q return None def __initialize_log(self): self.__log = { 'cpu': [], 'iter': [], 'f': [], 'status': [], 'sol_x': [], 'lam_x': [], 'lam_g': [], 'u0': [], 'nACtot': [], 'nAC': [], 'idx_AC': [], 'nAS': [] } self.__log_acados = { 'time_tot': [], 'time_lin': [], 'time_sim': [], 'time_qp': [], 'sqp_iter': [], 'time_reg': [], 'time_qp_xcond': [], 'time_qp_solver_call': [], } return None def __extract_solver_stats(self): info = self.__sqp_solver.stats self.__log['cpu'].append(info['t_wall_total']) self.__log['iter'].append(info['iter_count']) self.__log['status'].append(info['return_status']) self.__log['sol_x'].append(info['x']) self.__log['lam_g'].append(info['lam_g']) self.__log['f'].append(info['f']) self.__log['u0'].append(self.__w(info['x'])['u', 0]) self.__log['nACtot'].append(info['nAC']) nAC, idx_AC = self.__detect_AC(self.__g(info['lam_g'])) self.__log['nAC'].append(nAC) self.__log['idx_AC'].append(nAC) self.__log['nAS'].append(info['nAS']) return None def __extract_acados_solver_stats(self): for key in list(self.__log_acados.keys()): self.__log_acados[key].append( self.__acados_ocp_solver.get_stats(key)) return None def __detect_AC(self, lam_g_opt): # optimal active set if 'h' in lam_g_opt.keys(): idx_opt = [ k for k in range(self.__h.size1_out(0) - self.__nsc) if lam_g_opt['h', 0][k] != 0 ] lam_g_ref = self.__g(self.__ref_du[self.__index]) idx_ref = [ k for k in range(self.__h.size1_out(0) - self.__nsc) if lam_g_ref['h', 0][k] != 0 ] else: idx_opt = [] idx_ref = [] # get number of active set changes nAC = len([k for k in idx_opt if k not in idx_ref]) nAC += len([k for k in idx_ref if k not in idx_opt]) return nAC, idx_opt def reset(self): self.__index = 0 self.__index_acados = 0 self.__initialize_log() self.__set_initial_guess() return None def __shift_initial_guess(self, w0, lam_g0): w_shifted = self.__w(0.0) lam_g_shifted = self.__g(0.0) lam_g_shifted['init'] = lam_g0['dyn', 0] # shift states and controls for i in range(self.__N): # shift primal solution w_shifted['x', i] = w0['x', i + 1] if i < self.__N - 1: w_shifted['u', i] = w0['u', i + 1] if 'us' in self.__vars: w_shifted['us', i] = w0['us', i + 1] if 'usc' in self.__vars: w_shifted['usc', i] = w0['usc', i + 1] # shift dual solution lam_g_shifted['dyn', i] = lam_g0['dyn', i + 1] for constr in ['g', 'h']: if constr in lam_g0.keys(): lam_g_shifted[constr, i] = lam_g0[constr, i + 1] # copy final interval w_shifted['x', self.__N] = w_shifted['x', self.__N - 1] w_shifted['u', self.__N - 1] = w_shifted['u', self.__N - 2] if 'us' in self.__vars: w_shifted['us', self.__N - 1] = w_shifted['us', self.__N - 2] if 'usc' in self.__vars: w_shifted['usc', self.__N - 1] = w_shifted['usc', self.__N - 2] lam_g_shifted['dyn', self.__N - 1] = lam_g_shifted['dyn', self.__N - 2] for constr in ['g', 'h']: if constr in lam_g0.keys(): lam_g_shifted[constr, self.__N - 1] = lam_g_shifted[constr, self.__N - 2] lam_g_shifted['term'] = lam_g0['term'] return w_shifted, lam_g_shifted def __shift_initial_guess_acados(self): for i in range(self.__N): x_prev = np.squeeze(self.__w_sol_acados['x', i + 1].full(), axis=1) self.__acados_ocp_solver.set(i, "x", x_prev) if i < self.__N - 1: u_prev = np.squeeze(self.__w_sol_acados['u', i + 1].full(), axis=1) if 'us' in self.__vars: u_prev = np.squeeze(ct.vertcat( u_prev, self.__w_sol_acados['us', i + 1]).full(), axis=1) self.__acados_ocp_solver.set(i, "u", u_prev) # initial guess in terminal stage on periodic trajectory idx = (self.__index_acados + self.__N) % self.__Nref # reference xref = np.squeeze(self.__ref[(idx + 1) % self.__Nref][:self.__nx], axis=1) uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu + self.__ns], axis=1) self.__acados_ocp_solver.set(self.__N, "x", xref) self.__acados_ocp_solver.set(self.__N - 1, "u", uref) return None def __set_initial_guess(self): # create initial guess at steady state wref = self.__wref(self.__ref[self.__index]) w0 = self.__w(0.0) w0['x'] = wref['x'] w0['u'] = wref['u'] if 'us' in self.__vars: w0['us'] = wref['us'] self.__w0 = w0 # initial guess for multipliers self.__lam_g0 = self.__g(self.__ref_du[self.__index]) # acados solver initialization at reference if self.__acados_ocp_solver is not None: self.__set_acados_initial_guess() return None def __set_acados_reference(self): for i in range(self.__N): # periodic index idx = (self.__index_acados + i) % self.__Nref # reference xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1) uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu + self.__ns], axis=1) if self.__type == 'tracking': # construct output reference with gradient term yref = np.squeeze( ca.vertcat(xref,uref).full() - \ ct.mtimes( np.linalg.inv(self.__Href[idx][0]/self.__ts), # inverse of weighting matrix self.__qref[idx][0].T).full()/self.__ts, # gradient term axis = 1 ) self.__acados_ocp_solver.set(i, 'yref', yref) # update tuning matrix self.__acados_ocp_solver.cost_set( i, 'W', self.__Href[idx][0] / self.__ts) # set custom hessians if applicable # if self.__acados_ocp_solver.acados_ocp.solver_options.ext_cost_custom_hessian: # self.__acados_ocp_solver.cost_set(i, "cost_custom_hess", self.__custom_hessian[idx]) # update constraint bounds if self.__h is not None: C = self.__S['C'][idx][:, :self.__nx] D = self.__S['C'][idx][:, self.__nx:] lg = -self.__S['e'][idx] + ct.mtimes( C, xref).full() + ct.mtimes(D, uref).full() ug = 1e8 - self.__S['e'][idx] + ct.mtimes( C, xref).full() + ct.mtimes(D, uref).full() # remove constraints that depend on states only from first shooting node if i == 0: for k in range(D.shape[0]): if k in self.__h_us_idx + self.__h_x_idx: lg[k] += -1e8 self.__acados_ocp_solver.constraints_set( i, 'lg', np.squeeze(lg, axis=1)) self.__acados_ocp_solver.constraints_set( i, 'ug', np.squeeze(ug, axis=1)) # update terminal constraint idx = (self.__index_acados + self.__N) % self.__Nref x_term = np.squeeze(self.__p_operator(self.__ref[idx][:self.__nx]), axis=1) self.__acados_ocp_solver.set(self.__N, 'lbx', x_term) self.__acados_ocp_solver.set(self.__N, 'ubx', x_term) return None def __set_acados_initial_guess(self): # dual reference solution ref_dual = self.__ref_du_struct[self.__index_acados % self.__Nref] for i in range(self.__N): # periodic index idx = (self.__index_acados + i) % self.__Nref # initialize at reference xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1) uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu + self.__ns], axis=1) # set initial guess self.__acados_ocp_solver.set(i, "x", xref) self.__acados_ocp_solver.set(i, "u", uref) # set dual initial guess self.__acados_ocp_solver.set(i, "pi", np.squeeze(ref_dual['dyn', i].full())) # the inequalities are internally organized in the following order: # [ lbu lbx lg lh ubu ubx ug uh ] lam_h = [] t = [] if i == 0: lam_x0 = copy.deepcopy(ref_dual['init']) if 'h' in list(ref_dual.keys()): lam_lh0 = -ref_dual['h', i][:ref_dual['h', i].shape[0] - self.__nsc] t_lh0 = copy.deepcopy(self.__S['e'][idx % self.__Nref]) if i == 0: # set unused constraints at i=0 to be inactive C = self.__S['C'][idx][:, :self.__nx] D = self.__S['C'][idx][:, self.__nx:] for k in range(D.shape[0]): if k in self.__h_us_idx + self.__h_x_idx: lam_x0 += -ct.mtimes(lam_lh0[k], C[k, :]) lam_lh0[k] = 0.0 t_lh0[k] += 1e8 lam_lx0 = -copy.deepcopy(lam_x0) for k in range(self.__nx): if lam_lx0[k] < 0.0: lam_lx0[k] = 0.0 # assign multiplier to upper bound lam_h.append(lam_lx0) # lbx_0 t.append(np.zeros((self.__nx, ))) if 'h' in list(ref_dual.keys()): if i == 0: lam_lh = lam_lh0 t_lh = t_lh0 else: lam_lh = -ref_dual['h', i][:ref_dual['h', i].shape[0] - self.__nsc] t_lh = copy.deepcopy(self.__S['e'][idx % self.__Nref]) lam_h.append(lam_lh) # lg t.append(t_lh) if 'g' in list(ref_dual.keys()): lam_lg0 = -ref_dual['g', i] lam_ug0 = np.zeros(lam_lg0.shape) for k in range(lam_lg0.shape[0]): if lam_lg0[k] < 0.0: lam_ug0[k] = -lam_lg0[k] lam_lg0[k] = 0.0 lam_h.append(lam_lg0) # lh t.append(np.zeros((ref_dual['g', i].shape[0], ))) if i == 0: lam_ux0 = copy.deepcopy(lam_x0) for k in range(self.__nx): if lam_ux0[k] < 0.0: lam_ux0[k] = 0.0 # assign multiplier to lower bound lam_h.append(lam_ux0) # ubx_0 t.append(np.zeros((self.__nx, ))) if 'h' in list(ref_dual.keys()): lam_h.append( np.zeros((ref_dual['h', i].shape[0] - self.__nsc, ))) # ug t.append(1e8 * np.ones((ref_dual['h', i].shape[0] - self.__nsc, 1)) - self.__S['e'][idx]) if 'g' in list(ref_dual.keys()): lam_h.append(lam_ug0) # uh t.append(np.zeros((ref_dual['g', i].shape[0], ))) if self.__nsc > 0: lam_sl = self.__scost - ct.mtimes(lam_lh.T, self.__Jsg).T lam_h.append(lam_sl) # ls lam_h.append(self.__scost) # us t.append(np.zeros((self.__nsc, ))) # slg > 0 t.append(np.zeros((self.__nsc, ))) # sug > 0 if len(lam_h) != 0: self.__acados_ocp_solver.set( i, "lam", np.squeeze(ct.vertcat(*lam_h).full())) self.__acados_ocp_solver.set(i, "t", np.squeeze(ct.vertcat(*t).full())) # terminal state idx = (self.__index_acados + self.__N) % self.__Nref xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1) self.__acados_ocp_solver.set(self.__N, "x", xref) # terminal multipliers lam_lterm = -ref_dual['term'] lam_uterm = np.zeros((ref_dual['term'].shape[0], )) for k in range(lam_lterm.shape[0]): if lam_lterm[k] < 0.0: lam_uterm[k] = -lam_lterm[k] lam_lterm[k] = 0.0 lam_term = np.squeeze(ct.vertcat(lam_lterm, lam_uterm).full()) self.__acados_ocp_solver.set(self.__N, "lam", lam_term) return None def __detect_state_dependent_constraints(self): """ Detect which nonlinear equalities depend on states but not on controls. """ g_nl = self.__gnl(self.__vars['x'], self.__vars['u'], self.__vars['us']) self.__gnl_x_idx = [] for i in range(g_nl.shape[0]): if not True in ca.which_depends(g_nl[i], self.__vars['u'], 1): self.__gnl_x_idx.append(i) self.__h_us_idx = [ idx + self.__h.size1_out(0) - self.__ns for idx in self.__gnl_x_idx ] return None @property def w(self): return self.__w @property def g_sol(self): return self.__g_sol @property def w_sol(self): return self.__w_sol @property def log(self): return self.__log @property def log_acados(self): return self.__log_acados @property def index(self): return self.__index @property def acados_ocp_solver(self): return self.__acados_ocp_solver @property def acados_integrator(self): return self.__acados_integrator @property def w_sol_acados(self): return self.__w_sol_acados
def run_nominal_control(chain_params): # create ocp object to formulate the OCP ocp = AcadosOcp() # chain parameters n_mass = chain_params["n_mass"] M = chain_params["n_mass"] - 2 # number of intermediate masses Ts = chain_params["Ts"] Tsim = chain_params["Tsim"] N = chain_params["N"] u_init = chain_params["u_init"] with_wall = chain_params["with_wall"] yPosWall = chain_params["yPosWall"] m = chain_params["m"] D = chain_params["D"] L = chain_params["L"] perturb_scale = chain_params["perturb_scale"] nlp_iter = chain_params["nlp_iter"] nlp_tol = chain_params["nlp_tol"] save_results = chain_params["save_results"] show_plots = chain_params["show_plots"] seed = chain_params["seed"] np.random.seed(seed) nparam = 3*M W = perturb_scale * np.eye(nparam) # export model model = export_disturbed_chain_mass_model(n_mass, m, D, L) # set model ocp.model = model nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx Tf = N * Ts # initial state xPosFirstMass = np.zeros((3,1)) xEndRef = np.zeros((3,1)) xEndRef[0] = L * (M+1) * 6 pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass) xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef) x0 = xrest # set dimensions ocp.dims.N = N # set cost module ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' Q = 2*np.diagflat( np.ones((nx, 1)) ) q_diag = np.ones((nx,1)) strong_penalty = M+1 q_diag[3*M] = strong_penalty q_diag[3*M+1] = strong_penalty q_diag[3*M+2] = strong_penalty Q = 2*np.diagflat( q_diag ) R = 2*np.diagflat( 1e-2 * np.ones((nu, 1)) ) ocp.cost.W = scipy.linalg.block_diag(Q, R) ocp.cost.W_e = Q ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx,:nx] = np.eye(nx) Vu = np.zeros((ny, nu)) Vu[nx:nx+nu, :] = np.eye(nu) ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) # import pdb; pdb.set_trace() yref = np.vstack((xrest, np.zeros((nu,1)))).flatten() ocp.cost.yref = yref ocp.cost.yref_e = xrest.flatten() # set constraints umax = 1*np.ones((nu,)) ocp.constraints.constr_type = 'BGH' ocp.constraints.lbu = -umax ocp.constraints.ubu = umax ocp.constraints.x0 = x0.reshape((nx,)) ocp.constraints.idxbu = np.array(range(nu)) # disturbances nparam = 3*M ocp.parameter_values = np.zeros((nparam,)) # wall constraint if with_wall: nbx = M + 1 Jbx = np.zeros((nbx,nx)) for i in range(nbx): Jbx[i, 3*i+1] = 1.0 ocp.constraints.Jbx = Jbx ocp.constraints.lbx = yPosWall * np.ones((nbx,)) ocp.constraints.ubx = 1e9 * np.ones((nbx,)) # slacks ocp.constraints.Jsbx = np.eye(nbx) L2_pen = 1e3 L1_pen = 1 ocp.cost.Zl = L2_pen * np.ones((nbx,)) ocp.cost.Zu = L2_pen * np.ones((nbx,)) ocp.cost.zl = L1_pen * np.ones((nbx,)) ocp.cost.zu = L1_pen * np.ones((nbx,)) # solver options ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'IRK' ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI ocp.solver_options.nlp_solver_max_iter = nlp_iter ocp.solver_options.sim_method_num_stages = 2 ocp.solver_options.sim_method_num_steps = 2 ocp.solver_options.qp_solver_cond_N = N ocp.solver_options.qp_tol = nlp_tol ocp.solver_options.tol = nlp_tol # ocp.solver_options.nlp_solver_tol_eq = 1e-9 # set prediction horizon ocp.solver_options.tf = Tf acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') # acados_integrator = AcadosSimSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') acados_integrator = export_chain_mass_integrator(n_mass, m, D, L) #%% get initial state from xrest xcurrent = x0.reshape((nx,)) for i in range(5): acados_integrator.set("x", xcurrent) acados_integrator.set("u", u_init) status = acados_integrator.solve() if status != 0: raise Exception('acados integrator returned status {}. Exiting.'.format(status)) # update state xcurrent = acados_integrator.get("x") #%% actual simulation N_sim = int(np.floor(Tsim/Ts)) simX = np.ndarray((N_sim+1, nx)) simU = np.ndarray((N_sim, nu)) wall_dist = np.zeros((N_sim,)) timings = np.zeros((N_sim,)) simX[0,:] = xcurrent # closed loop for i in range(N_sim): # solve ocp acados_ocp_solver.set(0, "lbx", xcurrent) acados_ocp_solver.set(0, "ubx", xcurrent) status = acados_ocp_solver.solve() timings[i] = acados_ocp_solver.get_stats("time_tot")[0] if status != 0: raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i)) simU[i,:] = acados_ocp_solver.get(0, "u") print("control at time", i, ":", simU[i,:]) # simulate system acados_integrator.set("x", xcurrent) acados_integrator.set("u", simU[i,:]) pertubation = sampleFromEllipsoid(np.zeros((nparam,)), W) acados_integrator.set("p", pertubation) 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 # xOcpPredict = acados_ocp_solver.get(1, "x") # print("model mismatch = ", str(np.max(xOcpPredict - xcurrent))) yPos = xcurrent[range(1,3*M+1,3)] wall_dist[i] = np.min(yPos - yPosWall) print("time i = ", str(i), " dist2wall ", str(wall_dist[i])) print("dist2wall (minimum over simulation) ", str(np.min(wall_dist))) #%% plot results if os.environ.get('ACADOS_ON_TRAVIS') is None and show_plots: plot_chain_control_traj(simU) plot_chain_position_traj(simX, yPosWall=yPosWall) plot_chain_velocity_traj(simX) animate_chain_position(simX, xPosFirstMass, yPosWall=yPosWall) # animate_chain_position_3D(simX, xPosFirstMass) plt.show()