Hi 
Is it possible to use acados to “split” up an OCP in terms of QP iterations and solve it across N different solve calls?
Example: Assume that an OCP on average/worst-case requires 20 QP iterations to converge. If we set qp_iter_max=5 and call the solver 4 times sequentially, is it possible to get the same convergence? In other words, we want the iterations to continue from where they stopped the last time.
I have implemented a double integrator toy example (code attached below) with the python interface where the problem takes 4 QP iterations to converge:
Calling solver with qp_max_iter = 100
Qp iterations needed 4.0
Solver called 1 times until solution converged
------------
If I instead set qp_max_iter = 2 the same code gives the following:
Calling solver with qp_max_iter = 2
Qp iterations needed 2.0
Qp iterations needed 2.0
Qp iterations needed 2.0
Qp iterations needed 1.0
Solver called 4 times until solution converged
------------
I was expecting that since I do not set or update anything between the solver calls (primal & dual, solver internals should not change) the solution/iterations would just keep going from the previous solve call.
Question: Is what I’m trying to do possible in acados, or have I misunderstood some fundamental things regarding the solvers (HPIPM in this case). Or is there a potential bug that resets some internal variables of the solver?
Example code
import os
from types import SimpleNamespace
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
import numpy as np
from casadi import MX, vertcat
def get_system_model():
constraint = SimpleNamespace()
model = SimpleNamespace()
model_name = "ocp"
# State vector x
p = MX.sym("p")
v = MX.sym("v")
x = vertcat(p,v)
nx = x.size()[0]
# Input vector u
a = MX.sym("a")
u = vertcat(a)
nu = u.size()[0]
# x_dot
p_dot = MX.sym("p_dot")
v_dot = MX.sym("v_dot")
x_dot = vertcat(p_dot, v_dot)
# Algebraic variables
z = vertcat([])
# Parameters for the MPC problem
v_ref = MX.sym("v_ref")
p = vertcat(v_ref)
# dynamics
f_expl = vertcat(
v,
a
)
dx = vertcat(p, v - v_ref, a)
model.cost = sum( dx[i] ** 2 for i in range(nx+nu) )
dx_e = vertcat(p, v - v_ref)
model.cost_e = sum( dx_e[i] ** 2 for i in range(nx) )
# Set final model parameters
model.x0 = np.array([0, 0])
model.x = x
model.u = u
model.p = p
model.z = z
model.f_expl_expr = f_expl
model.f_impl_expr = x_dot - f_expl
model.xdot = x_dot
model.name = model_name
# Constraint on form h(x,u,p)
constraint.expr = vertcat(
v
)
constraint.expr_lb = np.array([-1])
constraint.expr_ub = np.array([30.0/3.6])
# Terminal constraint
constraint.expr_e = vertcat(
v
)
constraint.expr_lb_e = np.array([-1])
constraint.expr_ub_e = np.array([25.0/3.6])
# Slack variable setup
nsh = 0
ns = 0
# Upper and lower bounds on slack variables
constraint.lsh = 0 * np.ones(nsh)
constraint.ush = 0 * np.ones(nsh)
constraint.lsh_e = 0 * np.ones(nsh)
constraint.ush_e = 0 * np.ones(nsh)
# Indices to associate each slack variable to
constraint.idxsh = np.array([])
constraint.idxsh_e = np.array([])
# L1 weight for slack variables
model.slack_cost_zl = 5e2 * np.ones((ns))
model.slack_cost_zu = 5e2 * np.ones((ns))
model.slack_cost_zl_e = 5e2 * np.ones((ns))
model.slack_cost_zu_e = 5e2 * np.ones((ns))
# L2 weight for slack variables
model.slack_cost_Zl = 1e1 * np.ones((ns))
model.slack_cost_Zu = 1e1 * np.ones((ns))
model.slack_cost_Zl_e = 1e1 * np.ones((ns))
model.slack_cost_Zu_e = 1e1 * np.ones((ns))
# Statix box constraints
inf_val = 1e6
constraint.x_min = np.array(nx * [-inf_val])
constraint.x_max = np.array(nx * [inf_val])
constraint.u_min = np.array(nu * [-inf_val])
constraint.u_max = np.array(nu * [inf_val])
return model, constraint
def acados_settings(s_final, n_prediction, warm_start, qp_max_iter):
ocp = AcadosOcp()
model_ocp = AcadosModel()
model, constraint = get_system_model()
model_ocp.f_impl_expr = model.f_impl_expr
model_ocp.f_expl_expr = model.f_expl_expr
model_ocp.x = model.x
model_ocp.xdot = model.xdot
model_ocp.u = model.u
model_ocp.z = model.z
model_ocp.p = model.p
model_ocp.name = model.name
ocp.model = model_ocp
# Set lb <= h(x,u,p) <= ub stage constraint
model_ocp.con_h_expr = constraint.expr
ocp.constraints.lh = constraint.expr_lb
ocp.constraints.uh = constraint.expr_ub
# Set lb_e <= h_e(x,p) <= ub_e
model_ocp.con_h_expr_e = constraint.expr_e
ocp.constraints.lh_e = constraint.expr_lb_e
ocp.constraints.uh_e = constraint.expr_ub_e
ocp.constraints.lbx = constraint.x_min
ocp.constraints.ubx = constraint.x_max
ocp.constraints.idxbx = np.array([0, 1])
ocp.constraints.lbu = constraint.u_min
ocp.constraints.ubu = constraint.u_max
ocp.constraints.idxbu = np.array([0])
ocp.constraints.x0 = model.x0
ocp.constraints.lsh = constraint.lsh
ocp.constraints.ush = constraint.ush
ocp.constraints.idxsh = constraint.idxsh
ocp.constraints.lsh_e = constraint.lsh_e
ocp.constraints.ush_e = constraint.ush_e
ocp.constraints.idxsh_e = constraint.idxsh_e
model_ocp.cost_expr_ext_cost = model.cost
ocp.cost.cost_type = "EXTERNAL"
model_ocp.cost_expr_ext_cost_e = model.cost_e
ocp.cost.cost_type_e = "EXTERNAL"
ocp.cost.zl = model.slack_cost_zl
ocp.cost.zu = model.slack_cost_zu
ocp.cost.Zl = model.slack_cost_Zl
ocp.cost.Zu = model.slack_cost_Zu
ocp.cost.zl_e = model.slack_cost_zl_e
ocp.cost.zu_e = model.slack_cost_zu_e
ocp.cost.Zl_e = model.slack_cost_Zl_e
ocp.cost.Zu_e = model.slack_cost_Zu_e
# Solver settings
ocp.dims.N = n_prediction
ocp.solver_options.tf = s_final
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM"
ocp.solver_options.qp_solver_cond_N = 5
ocp.solver_options.nlp_solver_type = "SQP_RTI"
ocp.solver_options.hessian_approx = "EXACT"
ocp.solver_options.exact_hess_constr = 1
ocp.solver_options.exact_hess_cost = 1
ocp.solver_options.integrator_type = "ERK"
ocp.solver_options.sim_method_num_stages = 4
ocp.solver_options.sim_method_num_steps = 3
ocp.solver_options.qp_solver_warm_start = warm_start
ocp.solver_options.qp_solver_iter_max = qp_max_iter
# Default parameter value
ocp.parameter_values = np.array([25.0/3.6])
acados_solver = AcadosOcpSolver(ocp)
return model, constraint, acados_solver, ocp.parameter_values
def simulate_n_steps(ocp, params, x0, n_prediction):
for k in range(n_prediction + 1):
ocp.set(k, "p", params)
# Initial condition
ocp.set(0, "lbx", x0)
ocp.set(0, "ubx", x0)
# Solve
times_called = 0
while True:
status = ocp.solve()
times_called += 1
qp_iter = ocp.get_stats("qp_iter")[-1]
print(f"Qp iterations needed {qp_iter}")
if ocp.get_stats("statistics")[1][1] == 0:
break
print(f"Solver called {times_called} times until solution converged")
print("------------")
def main():
os.chdir(os.path.dirname(__file__))
# Generate MPC with prediction lenght t_T [s] and N shooting nodes
t_T = 1
t_N = 10
# Generate MPC formulation
warm_start = 2
qp_max_iter = 2
_, _, acados_solver, parameter_values = acados_settings(t_T, t_N, warm_start, qp_max_iter)
# Initialize state trajectory
x0 = np.array([0.0, 0.0])
for k in range(t_N + 1):
acados_solver.set(k, "x", x0)
print(f"Calling solver with qp_max_iter = {qp_max_iter}")
simulate_n_steps(acados_solver, parameter_values, x0, t_N)
if __name__ == "__main__":
main()
