for i in range(20): status = ocp_solver.solve() ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") residuals = ocp_solver.get_residuals() print("residuals after ", i, "SQP_RTI iterations:\n", residuals) if max(residuals) < tol: break if status != 0: raise Exception('acados returned status {}. Exiting.'.format(status)) # get solution for i in range(N): simX[i, :] = ocp_solver.get(i, "x") simU[i, :] = ocp_solver.get(i, "u") simX[N, :] = ocp_solver.get(N, "x") ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") # cost = ocp_solver.get_cost() print("cost function value of solution = ", cost) A = ocp_solver.dynamics_get(0, "A") print("forward sensitivities A at stage 0:", A) plot_pendulum(np.linspace(0, Tf, N + 1), Fmax, simU, simX, latexify=False)
# set prediction horizon ocp.solver_options.tf = Tf ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json') # ocp_solver.options_set("qp_solver_warm_start", 1) simX = np.ndarray((N + 1, nx)) simU = np.ndarray((N, nu)) status = ocp_solver.solve() if status != 0: ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") raise Exception('acados returned status {}. Exiting.'.format(status)) # get solution for i in range(N): simX[i, :] = ocp_solver.get(i, "x") simU[i, :] = ocp_solver.get(i, "u") simX[N, :] = ocp_solver.get(N, "x") ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") print("cost function value", ocp_solver.get_cost()) # plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False)