from acados_template import AcadosOcp, AcadosOcpSolver 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
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.")