# echo # echo "OCTAVE_PATH=$OCTAVE_PATH" # status = os.system( # "octave --eval \"convert_dae2gnsf({})\"".format("\'"+model.name+"_acados_dae.json\'") # ) # if status == 0: # print("\nsuccessfully detected GNSF structure in Octave\n") # else: # Exception("Failed to detect GNSF structure in Octave") # load gnsf from json with open(model.name + '_gnsf_functions.json', 'r') as f: import json gnsf_dict = json.load(f) ocp.gnsf_model = gnsf_dict # set prediction horizon ocp.solver_options.tf = Tf ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json') simX = np.ndarray((N + 1, nx)) simU = np.ndarray((N, nu)) status = ocp_solver.solve() if status != 0: raise Exception('acados returned status {}. Exiting.'.format(status))
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 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 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