# [00, 00, @2, 00, 00],
# [00, 00, 00, @1, 00],
# [00, 00, 00, 00, @1]])
# NOTE: hessian is wrt [u,x]
if EXTERNAL_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('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")
plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False)
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}')
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)
plot_pendulum(shooting_nodes, Fmax, simU, simX, latexify=False)
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 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")
xcurrent = x0
simX[0, :] = xcurrent
k_lin_feedback = 20 # use lin feedback k_lin_feedback -1 times
# closed loop
for i in range(Nsim):
if i % k_lin_feedback == 0:
# solve ocp
acados_ocp_solver.set(0, "lbx", xcurrent)
acados_ocp_solver.set(0, "ubx", xcurrent)
status = acados_ocp_solver.solve()
if status != 0:
print(xcurrent)
acados_ocp_solver.print_statistics()
raise Exception(
'acados acados_ocp_solver returned status {} in closed loop {}. Exiting.'
.format(status, i))
simU[i, :] = acados_ocp_solver.get(0, "u")
# calculate solution sensitivities
u_lin = simU[i, :]
x_lin = xcurrent
sens_u = np.ndarray((nu, nx))
sens_x = np.ndarray((nx, nx))
for index in range(nx):
acados_ocp_solver.eval_param_sens(index)
sens_u[:, index] = acados_ocp_solver.get(0, "sens_u")
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 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")
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 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
ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json')
# 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.')
# RESET
ocp_solver.reset()
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")