def create_ocp_solver(model, Tf, dt, kq, kr, n, m, max_abs_roll_rate, max_abs_pitch_rate, max_abs_yaw_rate, x0): ''' creates ACADOS ocp solver with conditions and costs ''' # create ocp object to formulate the OCP ocp = AcadosOcp() ocp.model = model nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx N = int(Tf / dt) ocp.dims.N = N # set cost Q = kq * np.eye(n) R = kr * np.eye(m) ocp.cost.W_e = Q ocp.cost.W = la.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[nx:, :] = np.eye(nu) ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) # solver options ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES # ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP_RTI' # SQP_RTI, SQP ocp.solver_options.tf = Tf # constraints on control ocp.constraints.lbu = np.array( [-max_abs_roll_rate, -max_abs_pitch_rate, -max_abs_pitch_rate, 0]) ocp.constraints.ubu = np.array( [max_abs_roll_rate, max_abs_pitch_rate, max_abs_pitch_rate, 1]) ocp.constraints.idxbu = np.array([0, 1, 2, 3]) ocp.constraints.x0 = x0 ocp.cost.yref = np.zeros((n + m, )) ocp.cost.yref_e = np.zeros((n, )) ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json') return ocp_solver
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 main(cost_type='NONLINEAR_LS', hessian_approximation='EXACT', ext_cost_use_num_hess=0, integrator_type='ERK'): print(f"using: cost_type {cost_type}, integrator_type {integrator_type}") # 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 ocp.dims.N = N # set cost Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) R = 2*np.diag([1e-2]) x = ocp.model.x u = ocp.model.u cost_W = scipy.linalg.block_diag(Q, R) if cost_type == 'LS': 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) elif cost_type == 'NONLINEAR_LS': ocp.cost.cost_type = 'NONLINEAR_LS' ocp.cost.cost_type_e = 'NONLINEAR_LS' ocp.model.cost_y_expr = vertcat(x, u) ocp.model.cost_y_expr_e = x elif cost_type == 'EXTERNAL': ocp.cost.cost_type = 'EXTERNAL' ocp.cost.cost_type_e = 'EXTERNAL' ocp.model.cost_expr_ext_cost = vertcat(x, u).T @ cost_W @ vertcat(x, u) ocp.model.cost_expr_ext_cost_e = x.T @ Q @ x else: raise Exception('Unknown cost_type. Possible values are \'LS\' and \'NONLINEAR_LS\'.') if cost_type in ['LS', 'NONLINEAR_LS']: ocp.cost.yref = np.zeros((ny, )) ocp.cost.yref_e = np.zeros((ny_e, )) ocp.cost.W_e = Q ocp.cost.W = cost_W # set constraints Fmax = 80 ocp.constraints.constr_type = 'BGH' 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 = hessian_approximation ocp.solver_options.regularize_method = 'CONVEXIFY' ocp.solver_options.integrator_type = integrator_type if ocp.solver_options.integrator_type == 'GNSF': import json with open('../getting_started/common/' + model.name + '_gnsf_functions.json', 'r') as f: gnsf_dict = json.load(f) ocp.gnsf_model = gnsf_dict 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_options.ext_cost_num_hess = ext_cost_use_num_hess # create solver 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') # time create Ncreate = 1 create_time = [] for i in range(Ncreate): t0 = time.time() # ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json', build=False, generate=False) 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) create_time.append( time.time() - t0) print(f"create_time: min {np.min(create_time)*1e3:.4f} ms mean {np.mean(create_time)*1e3:.4f} ms max {np.max(create_time)*1e3:.4f} ms over {Ncreate} executions") # create_time: min 0.2189 ms mean 0.2586 ms max 0.6881 ms over 10000 executions # RESET Nreset = 3 reset_time = [] for i in range(Nreset): # set NaNs as input to test reset() -> NOT RECOMMENDED!!! # ocp_solver.options_set('print_level', 2) for i in range(N): ocp_solver.set(i, 'x', np.NaN * np.ones((nx,))) ocp_solver.set(i, 'u', np.NaN * np.ones((nu,))) status = ocp_solver.solve() ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") if status == 0: raise Exception(f'acados returned status {status}, although NaNs were given.') else: print(f'acados returned status {status}, which is expected, since NaNs were given.') t0 = time.time() ocp_solver.reset() reset_time.append( time.time() - t0) print(f"reset_time: min {np.min(reset_time)*1e3:.4f} ms mean {np.mean(reset_time)*1e3:.4f} ms max {np.max(reset_time)*1e3:.4f} ms over {Nreset} executions") # OLD: without HPIPM mem reset: # reset_time: min 0.0019 ms mean 0.0022 ms max 0.0250 ms over 10000 executions # NEW: with HPIPM mem reset: # reset_time: min 0.0036 ms mean 0.0047 ms max 0.0906 ms over 10000 executions for i in range(N): ocp_solver.set(i, 'x', x0) if cost_type == 'EXTERNAL': # NOTE: hessian is wrt [u,x] if ext_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(f'acados returned status {status} for cost_type {cost_type}\n' f'integrator_type = {integrator_type}.') # 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")
def create_ocp_solver_separate_gains(model, Tf, dt, n, m, max_abs_roll_rate, max_abs_pitch_rate, max_abs_yaw_rate, x0, kqxy, kqz, kqrp, kqy, kqxyv, kqzv, krt, krrpr, kryr): # create ocp object to formulate the OCP ocp = AcadosOcp() ocp.model = model nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu # ny_e = nx N = int(Tf/dt) ocp.dims.N = N # set cost Q = np.eye(n) Q[0,0] = kqxy Q[1,1] = kqxy Q[2,2] = kqz Q[3,3] = kqrp Q[4,4] = kqrp Q[5,5] = kqy Q[6,6] = kqxyv Q[7,7] = kqxyv Q[8,8] = kqzv R = np.eye(m) R[0,0] = krrpr R[1,1] = krrpr R[2,2] = kryr R[3,3] = krt ocp.cost.W_e = Q ocp.cost.W = la.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[nx:,:] = np.eye(nu) ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) # solver options # ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP_RTI' # SQP_RTI, SQP ocp.solver_options.tf = Tf # constraints on control ocp.constraints.lbu = np.array([-max_abs_roll_rate, -max_abs_pitch_rate, -max_abs_pitch_rate,0]) ocp.constraints.ubu = np.array([max_abs_roll_rate, max_abs_pitch_rate, max_abs_pitch_rate,1]) ocp.constraints.idxbu = np.array([0,1,2,3]) ocp.constraints.x0 = x0 ocp.cost.yref = np.zeros((n+m,)) ocp.cost.yref_e = np.zeros((n,)) ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') return ocp_solver
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()
Tf = 1.5 # prediction horizon N = round(Tf * 20) # number of discretization steps T = 12.0 # maximum simulation time[s] mb = 4 mw = 2 mt = mb + mw Rw = 0.17 Iw = (mw * (Rw**2)) g = 9.81 car_model = CarModel_mpc_noise(mb, mw, Iw, Rw, Tf / N) model = car_model.model ocp = AcadosOcp() ocp.model = model print(model) # set dimensions nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx ocp.dims.N = N # set cost Q = np.diag([5, 50, 0.5, 1, 50, 500]) R = np.diag([10, 1]) Qe = np.diag([5, 50, 0.5, 1, 10, 500])
def acados_settings(Tf, N, track_file): # create render arguments ocp = AcadosOcp() # export model model, constraint = bicycle_model(track_file) # define acados ODE model_ac = AcadosModel() model_ac.f_impl_expr = model.f_impl_expr model_ac.f_expl_expr = model.f_expl_expr model_ac.x = model.x model_ac.xdot = model.xdot model_ac.u = model.u model_ac.z = model.z model_ac.p = model.p model_ac.name = model.name ocp.model = model_ac # define constraint model_ac.con_h_expr = constraint.expr # set dimensions nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx ocp.dims.N = N ns = 2 nsh = 2 # set cost Q = np.diag([1e-1, 1e-8, 1e-8, 1e-8, 1e-3, 5e-3]) R = np.eye(nu) R[0, 0] = 1e-3 R[1, 1] = 5e-3 Qe = np.diag([5e0, 1e1, 1e-8, 1e-8, 5e-3, 2e-3]) ocp.cost.cost_type = "LINEAR_LS" ocp.cost.cost_type_e = "LINEAR_LS" unscale = N / Tf ocp.cost.W = unscale * scipy.linalg.block_diag(Q, R) ocp.cost.W_e = Qe / unscale Vx = np.zeros((ny, nx)) Vx[:nx, :nx] = np.eye(nx) ocp.cost.Vx = Vx Vu = np.zeros((ny, nu)) Vu[6, 0] = 1.0 Vu[7, 1] = 1.0 ocp.cost.Vu = Vu Vx_e = np.zeros((ny_e, nx)) Vx_e[:nx, :nx] = np.eye(nx) ocp.cost.Vx_e = Vx_e ocp.cost.zl = 100 * np.ones((ns, )) ocp.cost.Zl = 0 * np.ones((ns, )) ocp.cost.zu = 100 * np.ones((ns, )) ocp.cost.Zu = 0 * np.ones((ns, )) # set intial references ocp.cost.yref = np.array([1, 0, 0, 0, 0, 0, 0, 0]) ocp.cost.yref_e = np.array([0, 0, 0, 0, 0, 0]) # setting constraints ocp.constraints.lbx = np.array([-12]) ocp.constraints.ubx = np.array([12]) ocp.constraints.idxbx = np.array([1]) ocp.constraints.lbu = np.array([model.dthrottle_min, model.ddelta_min]) ocp.constraints.ubu = np.array([model.dthrottle_max, model.ddelta_max]) ocp.constraints.idxbu = np.array([0, 1]) # ocp.constraints.lsbx=np.zero s([1]) # ocp.constraints.usbx=np.zeros([1]) # ocp.constraints.idxsbx=np.array([1]) ocp.constraints.lh = np.array([ constraint.along_min, constraint.alat_min, model.n_min, model.throttle_min, model.delta_min, ]) ocp.constraints.uh = np.array([ constraint.along_max, constraint.alat_max, model.n_max, model.throttle_max, model.delta_max, ]) ocp.constraints.lsh = np.zeros(nsh) ocp.constraints.ush = np.zeros(nsh) ocp.constraints.idxsh = np.array([0, 2]) # set intial condition ocp.constraints.x0 = model.x0 # set QP solver and integration ocp.solver_options.tf = Tf # ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" ocp.solver_options.nlp_solver_type = "SQP_RTI" ocp.solver_options.hessian_approx = "GAUSS_NEWTON" ocp.solver_options.integrator_type = "ERK" ocp.solver_options.sim_method_num_stages = 4 ocp.solver_options.sim_method_num_steps = 3 # ocp.solver_options.qp_solver_tol_stat = 1e-2 # ocp.solver_options.qp_solver_tol_eq = 1e-2 # ocp.solver_options.qp_solver_tol_ineq = 1e-2 # ocp.solver_options.qp_solver_tol_comp = 1e-2 # create solver acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json") return constraint, model, acados_solver
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 acados_settings(Tf, N): # create render arguments ocp = AcadosOcp() # export model model, constraint = usv_model() # define acados ODE model_ac = AcadosModel() model_ac.f_impl_expr = model.f_impl_expr model_ac.f_expl_expr = model.f_expl_expr model_ac.x = model.x model_ac.xdot = model.xdot model_ac.u = model.U model_ac.z = model.z model_ac.p = model.p model_ac.name = model.name ocp.model = model_ac # define constraint model_ac.con_h_expr = constraint.expr # set dimensions nx = model.x.size()[0] nu = model.U.size()[0] ny = nx + nu ny_e = nx ocp.dims.N = N ns = 8 nsh = 8 # set cost Q = np.diag([0, 0, 0.05, 0.01, 0, 0, 0, 0]) R = np.eye(nu) R[0, 0] = 0.2 Qe = np.diag([0, 0, 0.1, 0.05, 0, 0, 0, 0]) ocp.cost.cost_type = "LINEAR_LS" ocp.cost.cost_type_e = "LINEAR_LS" #unscale = N / Tf #ocp.cost.W = unscale * scipy.linalg.block_diag(Q, R) #ocp.cost.W_e = Qe / unscale ocp.cost.W = scipy.linalg.block_diag(Q, R) ocp.cost.W_e = Qe Vx = np.zeros((ny, nx)) Vx[:nx, :nx] = np.eye(nx) ocp.cost.Vx = Vx Vu = np.zeros((ny, nu)) Vu[8, 0] = 1.0 ocp.cost.Vu = Vu Vx_e = np.zeros((ny_e, nx)) Vx_e[:nx, :nx] = np.eye(nx) ocp.cost.Vx_e = Vx_e ocp.cost.zl = 10 * np.ones((ns, )) #previously 100 ocp.cost.Zl = 0 * np.ones((ns, )) ocp.cost.zu = 10 * np.ones((ns, )) #previously 100 ocp.cost.Zu = 0 * np.ones((ns, )) # set intial references ocp.cost.yref = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]) ocp.cost.yref_e = np.array([0, 0, 0, 0, 0, 0, 0, 0]) # setting constraints #ocp.constraints.lbx = np.array([model.psieddot_min]) #ocp.constraints.ubx = np.array([model.psieddot_max]) #ocp.constraints.idxbx = np.array([8]) ocp.constraints.lbu = np.array([model.Upsieddot_min]) ocp.constraints.ubu = np.array([model.Upsieddot_max]) ocp.constraints.idxbu = np.array([0]) # ocp.constraints.lsbx=np.zero s([1]) # ocp.constraints.usbx=np.zeros([1]) # ocp.constraints.idxsbx=np.array([1]) ocp.constraints.lh = np.array([ constraint.distance_min, constraint.distance_min, constraint.distance_min, constraint.distance_min, constraint.distance_min, constraint.distance_min, constraint.distance_min, constraint.distance_min ]) ocp.constraints.uh = np.array([ 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000 ]) '''ocp.constraints.lsh = np.zeros(nsh) ocp.constraints.ush = np.zeros(nsh) ocp.constraints.idxsh = np.array([0, 2])''' ocp.constraints.lsh = np.array( [-0.2, -0.2, -0.2, -0.2, -0.2, -0.2, -0.2, -0.2]) ocp.constraints.ush = np.array([0, 0, 0, 0, 0, 0, 0, 0]) ocp.constraints.idxsh = np.array([0, 1, 2, 3, 4, 5, 6, 7]) # set intial condition ocp.constraints.x0 = model.x0 #ocp.parameter_values = np.array([4,4,4,8,4,12,4,20,100,100,100,100,100,100,100,100]) ocp.parameter_values = np.array([ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100 ]) # set QP solver and integration ocp.solver_options.tf = Tf #ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" ocp.solver_options.nlp_solver_type = "SQP_RTI" #ocp.solver_options.nlp_solver_type = "SQP" ocp.solver_options.hessian_approx = "GAUSS_NEWTON" ocp.solver_options.integrator_type = "ERK" #ocp.solver_options.sim_method_num_stages = 4 #ocp.solver_options.sim_method_num_steps = 3 #ocp.solver_options.nlp_solver_max_iter = 200 #ocp.solver_options.tol = 1e-4 #ocp.solver_options.qp_solver_tol_stat = 1e-2 #ocp.solver_options.qp_solver_tol_eq = 1e-2 #ocp.solver_options.qp_solver_tol_ineq = 1e-2 #ocp.solver_options.qp_solver_tol_comp = 1e-2 # create solver acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json") return constraint, model, acados_solver
def acados_settings_dyn(Tf, N, modelparams = "modelparams.yaml"): #create render arguments ocp = AcadosOcp() #load model model, constraints = dynamic_model(modelparams) # define acados ODE model_ac = AcadosModel() model_ac.f_impl_expr = model.f_impl_expr model_ac.f_expl_expr = model.f_expl_expr model_ac.x = model.x model_ac.xdot = model.xdot #inputvector model_ac.u = model.u #parameter vector model_ac.p = model.p #parameter vector model_ac.z = model.z #external cost function model_ac.cost_expr_ext_cost = model.stage_cost #model_ac.cost_expr_ext_cost_e = 0 model_ac.con_h_expr = model.con_h_expr model_ac.name = model.name ocp.model = model_ac # set dimensions nx = model.x.size()[0] nu = model.u.size()[0] nz = model.z.size()[0] np = model.p.size()[0] #ny = nu + nx #ny_e = nx ocp.dims.nx = nx ocp.dims.nz = nz #ocp.dims.ny = ny #ocp.dims.ny_e = ny_e ocp.dims.nu = nu ocp.dims.np = np ocp.dims.nh = 1 #ocp.dims.ny = ny #ocp.dims.ny_e = ny_e ocp.dims.nbx = 3 ocp.dims.nsbx = 0 ocp.dims.nbu = nu #number of soft on h constraints ocp.dims.nsh = 1 ocp.dims.ns = 1 ocp.dims.N = N # set cost to casadi expression defined above ocp.cost.cost_type = "EXTERNAL" #ocp.cost.cost_type_e = "EXTERNAL" ocp.cost.zu = 1000 * npy.ones((ocp.dims.ns,)) ocp.cost.Zu = 1000 * npy.ones((ocp.dims.ns,)) ocp.cost.zl = 1000 * npy.ones((ocp.dims.ns,)) ocp.cost.Zl = 1000 * npy.ones((ocp.dims.ns,)) #not sure if needed #unscale = N / Tf #constraints #stagewise constraints for tracks with slack ocp.constraints.uh = npy.array([0.00]) ocp.constraints.lh = npy.array([-10]) ocp.constraints.lsh = 0.1*npy.ones(ocp.dims.nsh) ocp.constraints.ush = -0.1*npy.ones(ocp.dims.nsh) ocp.constraints.idxsh = npy.array([0]) #ocp.constraints.Jsh = 1 # boxconstraints # xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta'] ocp.constraints.lbx = npy.array([10, 10, 100, model.vx_min, model.vy_min, model.omega_min, model.d_min, model.delta_min, model.theta_min]) ocp.constraints.ubx = npy.array([10, 10, 100, model.vx_max, model.vy_max, model.omega_max, model.d_max, model.delta_max, model.theta_max]) ocp.constraints.idxbx = npy.arange(nx) #ocp.constraints.lsbx= -0.1 * npy.ones(ocp.dims.nbx) #ocp.constraints.usbx= 0.1 * npy.ones(ocp.dims.nbx) #ocp.constraints.idxsbx= npy.array([6,7,8]) #print("ocp.constraints.idxbx: ",ocp.constraints.idxbx) ocp.constraints.lbu = npy.array([model.ddot_min, model.deltadot_min, model.thetadot_min]) ocp.constraints.ubu = npy.array([model.ddot_max, model.deltadot_max, model.thetadot_max]) ocp.constraints.idxbu = npy.array([0, 1, 2]) # set intial condition ocp.constraints.x0 = model.x0 # set QP solver and integration ocp.solver_options.tf = Tf # ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" ocp.solver_options.nlp_solver_type = "SQP"#"SQP_RTI" # ocp.solver_options.hessian_approx = "GAUSS_NEWTON" ocp.solver_options.integrator_type = "ERK" ocp.parameter_values = npy.zeros(np) #ocp.solver_options.sim_method_num_stages = 4 #ocp.solver_options.sim_method_num_steps = 3 ocp.solver_options.nlp_solver_step_length = 0.01 ocp.solver_options.nlp_solver_max_iter = 50 ocp.solver_options.tol = 1e-4 #ocp.solver_options.print_level = 1 # ocp.solver_options.nlp_solver_tol_comp = 1e-1 # create solver acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp_dynamic.json") return constraints, model, acados_solver, ocp
def quadrotor_optimizer_setup(self, ): # Q_m_ = np.diag([80.0, 80.0, 120.0, 20.0, 20.0, # 30.0, 10.0, 10.0, 0.0]) # position, velocity, roll, pitch, (not yaw) # P_m_ = np.diag([86.21, 86.21, 120.95, # 6.94, 6.94, 11.04]) # only p and v # P_m_[0, 3] = 6.45 # P_m_[3, 0] = 6.45 # P_m_[1, 4] = 6.45 # P_m_[4, 1] = 6.45 # P_m_[2, 5] = 10.95 # P_m_[5, 2] = 10.95 # R_m_ = np.diag([50.0, 60.0, 1.0]) Q_m_ = np.diag( [ 10.0, 10.0, 10.0, 3e-1, 3e-1, 3e-1, #3e-1, 3e-1, 3e-2, 3e-2, 100.0, 100.0, 1e-3, 1e-3, 10.5, 10.5, 10.5 ] ) # position, velocity, load_position, load_velocity, [roll, pitch, yaw] P_m_ = np.diag([ 10.0, 10.0, 10.0, 0.05, 0.05, 0.05 # 10.0, 10.0, 10.0, # 0.05, 0.05, 0.05 ]) # only p and v # P_m_[0, 8] = 6.45 # P_m_[8, 0] = 6.45 # P_m_[1, 9] = 6.45 # P_m_[9, 1] = 6.45 # P_m_[2, 10] = 10.95 # P_m_[10, 2] = 10.95 R_m_ = np.diag([3.0, 3.0, 3.0, 1.0]) nx = self.model.x.size()[0] self.nx = nx nu = self.model.u.size()[0] self.nu = nu ny = nx + nu n_params = self.model.p.size()[0] if isinstance(self.model.p, ca.SX) else 0 acados_source_path = os.environ['ACADOS_SOURCE_DIR'] sys.path.insert(0, acados_source_path) # create OCP ocp = AcadosOcp() ocp.acados_include_path = acados_source_path + '/include' ocp.acados_lib_path = acados_source_path + '/lib' ocp.model = self.model ocp.dims.N = self.N ocp.solver_options.tf = self.T # initialize parameters ocp.dims.np = n_params ocp.parameter_values = np.zeros(n_params) # cost type ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.W = scipy.linalg.block_diag(Q_m_, R_m_) ocp.cost.W_e = P_m_ # np.zeros((nx-3, nx-3)) ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) ocp.cost.Vu = np.zeros((ny, nu)) ocp.cost.Vu[-nu:, -nu:] = np.eye(nu) ocp.cost.Vx_e = np.zeros((nx - 7, nx)) # only consider p and v ocp.cost.Vx_e[:nx - 7, :nx - 7] = np.eye(nx - 7) # initial reference trajectory_ref x_ref = np.zeros(nx) x_ref_e = np.zeros(nx - 7) u_ref = np.zeros(nu) u_ref[-1] = self.g ocp.cost.yref = np.concatenate((x_ref, u_ref)) ocp.cost.yref_e = x_ref_e # Set constraints ocp.constraints.lbu = np.array([ self.constraints.roll_min, self.constraints.pitch_min, self.constraints.yaw_min, self.constraints.thrust_min ]) ocp.constraints.ubu = np.array([ self.constraints.roll_max, self.constraints.pitch_max, self.constraints.yaw_max, self.constraints.thrust_max ]) ocp.constraints.idxbu = np.array([0, 1, 2, 3]) # initial state ocp.constraints.x0 = x_ref # solver options ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # explicit Runge-Kutta integrator ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP' # 'SQP_RTI' ocp.solver_options.levenberg_marquardt = 0.12 # 0.0 # compile acados ocp json_file = os.path.join('./' + self.model.name + '_acados_ocp.json') self.solver = AcadosOcpSolver(ocp, json_file=json_file) if self.simulation_required: self.integrator = AcadosSimSolver(ocp, json_file=json_file)
def generate(self, dae, 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) # 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 n_in = dae.n_in() if n_in == 2: # xdot = f(x, u) if 'integrator_type' in opts: if opts['integrator_type'] == 'IRK': 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': 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 # 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' else: # set weighting matrices if self.__type == 'tracking': 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)) # 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) ocp.cost.zu = np.squeeze(self.__scost.full(), axis=1) # 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()): 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 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")
def quadrotor_optimizer_setup(self, ): Q_m_ = np.diag([ 10, 10, 10, 0.3, 0.3, 0.3, 0.3, 0.05, 0.05, 0.05, ]) # position, q, v P_m_ = np.diag([10, 10, 10, 0.05, 0.05, 0.05]) # only p and v R_m_ = np.diag([5.0, 5.0, 5.0, 0.6]) nx = self.model.x.size()[0] self.nx = nx nu = self.model.u.size()[0] self.nu = nu ny = nx + nu n_params = self.model.p.size()[0] if isinstance(self.model.p, ca.SX) else 0 acados_source_path = os.environ['ACADOS_SOURCE_DIR'] sys.path.insert(0, acados_source_path) # create OCP ocp = AcadosOcp() ocp.acados_include_path = acados_source_path + '/include' ocp.acados_lib_path = acados_source_path + '/lib' ocp.model = self.model ocp.dims.N = self.N ocp.solver_options.tf = self.T # initialize parameters ocp.dims.np = n_params ocp.parameter_values = np.zeros(n_params) # cost type ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.W = scipy.linalg.block_diag(Q_m_, R_m_) ocp.cost.W_e = P_m_ ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) ocp.cost.Vu = np.zeros((ny, nu)) ocp.cost.Vu[-nu:, -nu:] = np.eye(nu) ocp.cost.Vx_e = np.zeros((nx - 4, nx)) # ocp.cost.Vx_e[:6, :6] = np.eye(6) ocp.cost.Vx_e[:3, :3] = np.eye(3) ocp.cost.Vx_e[-3:, -3:] = np.eye(3) # initial reference trajectory_ref x_ref = np.zeros(nx) x_ref[3] = 1.0 x_ref_e = np.zeros(nx - 4) u_ref = np.zeros(nu) u_ref[-1] = self.g_ ocp.cost.yref = np.concatenate((x_ref, u_ref)) ocp.cost.yref_e = x_ref_e # Set constraints ocp.constraints.lbu = np.array([ self.constraints.roll_rate_min, self.constraints.pitch_rate_min, self.constraints.yaw_rate_min, self.constraints.thrust_min ]) ocp.constraints.ubu = np.array([ self.constraints.roll_rate_max, self.constraints.pitch_rate_max, self.constraints.yaw_rate_max, self.constraints.thrust_max ]) ocp.constraints.idxbu = np.array([0, 1, 2, 3]) # initial state ocp.constraints.x0 = x_ref # solver options ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # explicit Runge-Kutta integrator ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP' # 'SQP_RTI' ocp.solver_options.nlp_solver_max_iter = 400 # compile acados ocp ## files are stored in .ros/ json_file = os.path.join('./' + self.model.name + '_acados_ocp.json') self.solver = AcadosOcpSolver(ocp, json_file=json_file) if self.simulation_required: self.integrator = AcadosSimSolver(ocp, json_file=json_file)
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}')
def main(use_cython=True): # (very) simple crane model beta = 0.001 k = 0.9 a_max = 10 dt_max = 2.0 # states p1 = SX.sym('p1') v1 = SX.sym('v1') p2 = SX.sym('p2') v2 = SX.sym('v2') x = vertcat(p1, v1, p2, v2) # controls a = SX.sym('a') dt = SX.sym('dt') u = vertcat(a, dt) f_expl = dt * vertcat(v1, a, v2, -beta * v2 - k * (p2 - p1)) model = AcadosModel() model.f_expl_expr = f_expl model.x = x model.u = u model.name = 'crane_time_opt' # create ocp object to formulate the OCP x0 = np.array([2.0, 0.0, 2.0, 0.0]) xf = np.array([0.0, 0.0, 0.0, 0.0]) ocp = AcadosOcp() ocp.model = model # N - maximum number of bangs N = 7 Tf = N nx = model.x.size()[0] nu = model.u.size()[0] # set dimensions ocp.dims.N = N # set cost ocp.cost.cost_type = 'EXTERNAL' ocp.cost.cost_type_e = 'EXTERNAL' ocp.model.cost_expr_ext_cost = dt ocp.model.cost_expr_ext_cost_e = 0 ocp.constraints.lbu = np.array([-a_max, 0.0]) ocp.constraints.ubu = np.array([+a_max, dt_max]) ocp.constraints.idxbu = np.array([0, 1]) ocp.constraints.x0 = x0 ocp.constraints.lbx_e = xf ocp.constraints.ubx_e = xf ocp.constraints.idxbx_e = np.array([0, 1, 2, 3]) # set prediction horizon ocp.solver_options.tf = Tf # set options ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' #'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 3 ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP ocp.solver_options.globalization = 'MERIT_BACKTRACKING' ocp.solver_options.nlp_solver_max_iter = 5000 ocp.solver_options.nlp_solver_tol_stat = 1e-6 ocp.solver_options.levenberg_marquardt = 0.1 ocp.solver_options.sim_method_num_steps = 15 ocp.solver_options.qp_solver_iter_max = 100 ocp.code_export_directory = 'c_generated_code' ocp.solver_options.hessian_approx = 'EXACT' ocp.solver_options.exact_hess_constr = 0 ocp.solver_options.exact_hess_dyn = 0 if use_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') else: # ctypes ## Note: skip generate and build assuming this is done before (in cython run) ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json', build=False, generate=False) ocp_solver.reset() for i, tau in enumerate(np.linspace(0, 1, N)): ocp_solver.set(i, 'x', (1 - tau) * x0 + tau * xf) ocp_solver.set(i, 'u', np.array([0.1, 0.5])) simX = np.zeros((N + 1, nx)) simU = np.zeros((N, nu)) status = ocp_solver.solve() if status != 0: ocp_solver.print_statistics() raise Exception(f'acados returned status {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") dts = simU[:, 1] print( "acados solved OCP successfully, creating integrator to simulate the solution" ) # simulate on finer grid sim = AcadosSim() # set model sim.model = model # set options sim.solver_options.integrator_type = 'ERK' sim.solver_options.num_stages = 4 sim.solver_options.num_steps = 3 sim.solver_options.T = 1.0 # dummy value dt_approx = 0.0005 dts_fine = np.zeros((N, )) Ns_fine = np.zeros((N, ), dtype='int16') # compute number of simulation steps for bang interval + dt_fine for i in range(N): N_approx = max(int(dts[i] / dt_approx), 1) dts_fine[i] = dts[i] / N_approx Ns_fine[i] = int(round(dts[i] / dts_fine[i])) N_fine = int(np.sum(Ns_fine)) simU_fine = np.zeros((N_fine, nu)) ts_fine = np.zeros((N_fine + 1, )) simX_fine = np.zeros((N_fine + 1, nx)) simX_fine[0, :] = x0 acados_integrator = AcadosSimSolver(sim) k = 0 for i in range(N): u = simU[i, 0] acados_integrator.set("u", np.hstack((u, np.ones(1, )))) # set simulation time acados_integrator.set("T", dts_fine[i]) for j in range(Ns_fine[i]): acados_integrator.set("x", simX_fine[k, :]) status = acados_integrator.solve() if status != 0: raise Exception(f'acados returned status {status}.') simX_fine[k + 1, :] = acados_integrator.get("x") simU_fine[k, :] = u ts_fine[k + 1] = ts_fine[k] + dts_fine[i] k += 1 # visualize if os.environ.get('ACADOS_ON_TRAVIS'): plt.figure() state_labels = ['p1', 'v1', 'p2', 'v2'] for i, l in enumerate(state_labels): plt.subplot(5, 1, i + 1) plt.plot(ts_fine, simX_fine[:, i], label='time optimal solution') plt.grid(True) plt.ylabel(l) if i == 0: plt.legend(loc=1) plt.subplot(5, 1, 5) plt.step(ts_fine, np.hstack((simU_fine[:, 0], simU_fine[-1, 0])), '-', where='post') plt.grid(True) plt.ylabel('a') plt.xlabel('t') plt.show()
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 # NOTE: all dimensions but N ar detected ocp.dims.N = N # 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]) 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]) # 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' ocp.solver_options.tol = 1e-1 * tol 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 # get slack values at stage 1 sl = acados_ocp_solver.get(1, "sl") su = acados_ocp_solver.get(1, "su") print("sl", sl, "su", su) # plot results # plot_pendulum(np.linspace(0, Tf, N+1), 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.")
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")
from export_pendulum_ode_model import export_pendulum_ode_model import numpy as np import scipy.linalg from utils import plot_pendulum from casadi import vertcat COST_MODULE = 'EXTERNAL' # 'LS', 'EXTERNAL' HESSIAN_APPROXIMATION = 'EXACT' # 'GAUSS_NEWTON EXTERNAL_COST_USE_NUM_HESS = 1 # 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.nbu = nu ocp.dims.nu = nu ocp.dims.N = N
def __init__(self, quad, t_horizon=1, n_nodes=20, q_cost=None, r_cost=None, q_mask=None, B_x=None, gp_regressors=None, rdrv_d_mat=None, model_name="quad_3d_acados_mpc", solver_options=None): """ :param quad: quadrotor object :type quad: Quadrotor3D :param t_horizon: time horizon for MPC optimization :param n_nodes: number of optimization nodes until time horizon :param q_cost: diagonal of Q matrix for LQR cost of MPC cost function. Must be a numpy array of length 12. :param r_cost: diagonal of R matrix for LQR cost of MPC cost function. Must be a numpy array of length 4. :param B_x: dictionary of matrices that maps the outputs of the gp regressors to the state space. :param gp_regressors: Gaussian Process ensemble for correcting the nominal model :type gp_regressors: GPEnsemble :param q_mask: Optional boolean mask that determines which variables from the state compute towards the cost function. In case no argument is passed, all variables are weighted. :param solver_options: Optional set of extra options dictionary for solvers. :param rdrv_d_mat: 3x3 matrix that corrects the drag with a linear model according to Faessler et al. 2018. None if not used """ # Weighted squared error loss function q = (p_xyz, a_xyz, v_xyz, r_xyz), r = (u1, u2, u3, u4) if q_cost is None: q_cost = np.array([10, 10, 10, 0.1, 0.1, 0.1, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05]) if r_cost is None: r_cost = np.array([0.1, 0.1, 0.1, 0.1]) self.T = t_horizon # Time horizon self.N = n_nodes # number of control nodes within horizon self.quad = quad self.max_u = quad.max_input_value self.min_u = quad.min_input_value # Declare model variables self.p = cs.MX.sym('p', 3) # position self.q = cs.MX.sym('a', 4) # angle quaternion (wxyz) self.v = cs.MX.sym('v', 3) # velocity self.r = cs.MX.sym('r', 3) # angle rate # Full state vector (13-dimensional) self.x = cs.vertcat(self.p, self.q, self.v, self.r) self.state_dim = 13 # Control input vector u1 = cs.MX.sym('u1') u2 = cs.MX.sym('u2') u3 = cs.MX.sym('u3') u4 = cs.MX.sym('u4') self.u = cs.vertcat(u1, u2, u3, u4) # Nominal model equations symbolic function (no GP) self.quad_xdot_nominal = self.quad_dynamics(rdrv_d_mat) # Linearized model dynamics symbolic function self.quad_xdot_jac = self.linearized_quad_dynamics() # Initialize objective function, 0 target state and integration equations self.L = None self.target = None # Check if GP ensemble has an homogeneous feature space (if actual Ensemble) if gp_regressors is not None and gp_regressors.homogeneous: self.gp_reg_ensemble = gp_regressors self.B_x = [B_x[dim] for dim in gp_regressors.dim_idx] self.B_x = np.squeeze(np.stack(self.B_x, axis=1)) self.B_x = self.B_x[:, np.newaxis] if len(self.B_x.shape) == 1 else self.B_x self.B_z = gp_regressors.B_z elif gp_regressors and gp_regressors.no_ensemble: # If not homogeneous, we have to treat each z feature vector independently self.gp_reg_ensemble = gp_regressors self.B_x = [B_x[dim] for dim in gp_regressors.dim_idx] self.B_x = np.squeeze(np.stack(self.B_x, axis=1)) self.B_x = self.B_x[:, np.newaxis] if len(self.B_x.shape) == 1 else self.B_x self.B_z = gp_regressors.B_z else: self.gp_reg_ensemble = None # Declare model variables for GP prediction (only used in real quadrotor flight with EKF estimator). # Will be used as initial state for GP prediction as Acados parameters. self.gp_p = cs.MX.sym('gp_p', 3) self.gp_q = cs.MX.sym('gp_a', 4) self.gp_v = cs.MX.sym('gp_v', 3) self.gp_r = cs.MX.sym('gp_r', 3) self.gp_x = cs.vertcat(self.gp_p, self.gp_q, self.gp_v, self.gp_r) # The trigger variable is used to tell ACADOS to use the additional GP state estimate in the first optimization # node and the regular integrated state in the rest self.trigger_var = cs.MX.sym('trigger', 1) # Build full model. Will have 13 variables. self.dyn_x contains the symbolic variable that # should be used to evaluate the dynamics function. It corresponds to self.x if there are no GP's, or # self.x_with_gp otherwise acados_models, nominal_with_gp = self.acados_setup_model( self.quad_xdot_nominal(x=self.x, u=self.u)['x_dot'], model_name) # Convert dynamics variables to functions of the state and input vectors self.quad_xdot = {} for dyn_model_idx in nominal_with_gp.keys(): dyn = nominal_with_gp[dyn_model_idx] self.quad_xdot[dyn_model_idx] = cs.Function('x_dot', [self.x, self.u], [dyn], ['x', 'u'], ['x_dot']) # ### Setup and compile Acados OCP solvers ### # self.acados_ocp_solver = {} # Check if GP's have been loaded self.with_gp = self.gp_reg_ensemble is not None # Add one more weight to the rotation (use quaternion norm weighting in acados) q_diagonal = np.concatenate((q_cost[:3], np.mean(q_cost[3:6])[np.newaxis], q_cost[3:])) if q_mask is not None: q_mask = np.concatenate((q_mask[:3], np.zeros(1), q_mask[3:])) q_diagonal *= q_mask # Ensure current working directory is current folder os.chdir(os.path.dirname(os.path.realpath(__file__))) self.acados_models_dir = '../../acados_models' safe_mkdir_recursive(os.path.join(os.getcwd(), self.acados_models_dir)) for key, key_model in zip(acados_models.keys(), acados_models.values()): nx = key_model.x.size()[0] nu = key_model.u.size()[0] ny = nx + nu n_param = key_model.p.size()[0] if isinstance(key_model.p, cs.MX) else 0 acados_source_path = os.environ['ACADOS_SOURCE_DIR'] sys.path.insert(0, '../common') # Create OCP object to formulate the optimization ocp = AcadosOcp() ocp.acados_include_path = acados_source_path + '/include' ocp.acados_lib_path = acados_source_path + '/lib' ocp.model = key_model ocp.dims.N = self.N ocp.solver_options.tf = t_horizon # Initialize parameters ocp.dims.np = n_param ocp.parameter_values = np.zeros(n_param) ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.W = np.diag(np.concatenate((q_diagonal, r_cost))) ocp.cost.W_e = np.diag(q_diagonal) terminal_cost = 0 if solver_options is None or not solver_options["terminal_cost"] else 1 ocp.cost.W_e *= terminal_cost ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) ocp.cost.Vu = np.zeros((ny, nu)) ocp.cost.Vu[-4:, -4:] = np.eye(nu) ocp.cost.Vx_e = np.eye(nx) # Initial reference trajectory (will be overwritten) x_ref = np.zeros(nx) ocp.cost.yref = np.concatenate((x_ref, np.array([0.0, 0.0, 0.0, 0.0]))) ocp.cost.yref_e = x_ref # Initial state (will be overwritten) ocp.constraints.x0 = x_ref # Set constraints ocp.constraints.lbu = np.array([self.min_u] * 4) ocp.constraints.ubu = np.array([self.max_u] * 4) ocp.constraints.idxbu = np.array([0, 1, 2, 3]) # Solver options ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP_RTI' if solver_options is None else solver_options["solver_type"] # Compile acados OCP solver if necessary json_file = os.path.join(self.acados_models_dir, key_model.name + '_acados_ocp.json') self.acados_ocp_solver[key] = AcadosOcpSolver(ocp, json_file=json_file)