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)
