I would like to ask about the process to reuse the C generated code. My setup, robot, dynamics, constraints and so on do not change but yref, initial conditions, and parameters do change.
Below is my setup function. I did some trial and error and it seems to error out if I do not add self.ocp_solver.reset() at the end of the setup. Before I call the self.ocp_solver.solve() I do set the lbx, ubx, p and yref.
def setup(self, config, collision_config):
mpc_config = config["mpc"]
Fmax = mpc_config["Fmax"]
N_horizon = mpc_config["N_horizon"]
Tf = N_horizon * mpc_config["mpc_timestep"] # Time horizon
# Create ocp object to formulate the OCP
ocp = AcadosOcp()
# Call model creation function
model, self.robot_sys = export_ode_model(config)
ocp.model = model
# Extract state and input dimensions
nx = model.x.rows()
nu = model.u.rows()
# Generate collision constraints
if collision_config is not None and config["collision"]["collision_avoidance"]:
self.add_hard_constraints(ocp, model, collision_config)
# Set stage cost module
self.define_stage_cost(ocp, model, config)
# Set terminal cost module
if self.terminal_cost:
self.define_terminal_cost(ocp, model, config)
# Set input constraints
ocp.constraints.lbu = -np.array(Fmax)
ocp.constraints.ubu = np.array(Fmax)
# Apply above to all inputs
ocp.constraints.idxbu = np.arange(nu)
# Set initial constraint
ocp.constraints.x0 = self.x0
# set prediction horizon
ocp.solver_options.N_horizon = N_horizon
ocp.solver_options.tf = Tf # Total predicton time
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'IRK'
ocp.solver_options.sim_method_newton_iter = 10
ocp.solver_options.regularize_method = mpc_config["regularize_method"] # For the Hessian
ocp.solver_options.levenberg_marquardt = mpc_config["levenberg_marquardt"]
ocp.solver_options.nlp_solver_warm_start_first_qp_from_nlp = mpc_config["nlp_solver_warm_start_first_qp_from_nlp"]
ocp.solver_options.nlp_solver_warm_start_first_qp = mpc_config["nlp_solver_warm_start_first_qp"]
ocp.solver_options.qp_solver_warm_start = mpc_config["qp_solver_warm_start"]
if self.use_RTI:
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
else:
ocp.solver_options.nlp_solver_type = 'SQP'
ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization
ocp.solver_options.nlp_solver_max_iter = 6
ocp.solver_options.qp_solver_cond_N = N_horizon
ocp.solver_options.nlp_solver_tol_stat = 1e-3
# ocp.solver_options.qp_solver_iter_max
# Create solver based on settings above
solver_json = 'acados_ocp_' + model.name + '.json'
self.ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json, verbose=False, build=False, generate=False)
I think I can also skip all the steps before self.ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json, verbose=False, build=False, generate=False).
Is this the correct method to reuse the C generated code? And if there are some examples, please do let me know.
I am not sure what the question is.
However, maybe the following clarifies some things:
creating an AcadosOcpSolver without build=False, generate=False results in identical solver as previously created, before any interactions with the solver were done (get,set, etc).
Note that a PR will be merged soon that automatically sets build and generate to True if the formulation changed: Python: implement check_reuse_possible by FreyJo · Pull Request #1763 · acados/acados · GitHub
reset() restores the iterate to what was there when the solver was created. But does not change problem data (p, lbx, yref etc).
Sorry that I wasn’t clear about the question but yes I am asking about the correct process to reuse the solver previously created and what I might need to set/reset before using it again. This is my modified workflow, I check for an existing correctly named JSON, and if so I assume that it is correct (Its not full proof, but good enough for me). And if it does exist then I reset the iterate.
So far I have only had an issue if I change the weights inside self.define_stage_cost(ocp, model, config), which I am guessing that since it does not build/generate then it is not updated.
But anyways I look forward to the update that automatically checks, it will save a lot of compilation time.
Thanks
Nico
def setup(self, config, collision_config):
mpc_config = config["mpc"]
Fmax = mpc_config["Fmax"]
N_horizon = mpc_config["N_horizon"]
Tf = N_horizon * mpc_config["mpc_timestep"] # Time horizon
# Create ocp object to formulate the OCP
ocp = AcadosOcp()
# Call model creation function
model, self.robot_sys = export_ode_model(config)
ocp.model = model
# Extract state and input dimensions
nx = model.x.rows()
nu = model.u.rows()
# Generate collision constraints
if collision_config is not None and config["collision"]["collision_avoidance"]:
self.add_hard_constraints(ocp, model, collision_config)
# Set stage cost module
self.define_stage_cost(ocp, model, config)
# Set terminal cost module
if self.terminal_cost:
self.define_terminal_cost(ocp, model, config)
# Set input constraints
ocp.constraints.lbu = -np.array(Fmax)
ocp.constraints.ubu = np.array(Fmax)
# Apply above to all inputs
ocp.constraints.idxbu = np.arange(nu)
# Set initial constraint
ocp.constraints.x0 = self.x0
# set prediction horizon
ocp.solver_options.N_horizon = N_horizon
ocp.solver_options.tf = Tf # Total predicton time
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'IRK'
ocp.solver_options.sim_method_newton_iter = 10
ocp.solver_options.regularize_method = mpc_config["regularize_method"] # For the Hessian
ocp.solver_options.levenberg_marquardt = mpc_config["levenberg_marquardt"]
ocp.solver_options.nlp_solver_warm_start_first_qp_from_nlp = mpc_config["nlp_solver_warm_start_first_qp_from_nlp"]
ocp.solver_options.nlp_solver_warm_start_first_qp = mpc_config["nlp_solver_warm_start_first_qp"]
ocp.solver_options.qp_solver_warm_start = mpc_config["qp_solver_warm_start"]
if self.use_RTI:
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
else:
ocp.solver_options.nlp_solver_type = 'SQP'
ocp.solver_options.globalization = 'MERIT_BACKTRACKING' # turns on globalization
ocp.solver_options.nlp_solver_max_iter = 6
ocp.solver_options.qp_solver_cond_N = N_horizon
ocp.solver_options.nlp_solver_tol_stat = 1e-4
# ocp.solver_options.qp_solver_iter_max
# Create solver based on settings above
solver_json = 'acados_ocp_' + self.config["model"]["name"] + '.json'
if self.check_for_existing_json(solver_json):
self.ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json, verbose=False, build=False, generate=False)
self.ocp_solver.reset()
else:
self.ocp_solver = AcadosOcpSolver(ocp, json_file = solver_json, verbose=False)
I tried out the new check_reuse_possible flag and it works quite nicely! Thanks!
However, I did find the case of changing the x0 conditions will cause it to rebuild, is there a way to circumvent this? I managed to trick the checks by setting an inital x0 to a static value and when it passed the checks then use constraints_set to set it to the actual x0 for warm up. This works to a certain extent but when the actual x0 is far from the static value, the QP solver throws the min_step error. Any help is appreciated.
when the solver is created, we initialize the x trajectory with the provided x0. To get the same behaviour after updating the initial state constraint, you need to set the initial guess for the x trajectory using set or set_flat accordingly.
Thanks for the information, it works as expected after using set_flat to update the initial guess. Related to this topic, I have another issue. I have 2 concurrent controllers in acados. The first with a shorter horizon starting from x0 and ending at the terminal state xN. Then this terminal state is fed to the second controller. xN in my case can change dramatically at each timestep as MPC tries to stablize the system and can cause the second controller to fail. I have tried to reset() the QP solver, and also set_flat both x and u with xN and zeros before calling it but still observe failure quite often. I was wondering if there is some advice to give the solver a higher chance of solving?
Have you had a look at the multi-phase formulation implemented in acados? I guess this allows you to implement both problems within a single multi-phase OCP.
No I have not looked at this before but I just briefly looked at it. Indeed I am trying to achieve the same goal of an infinite horizon but in my case, with a neural network (with information of the terminal state). Hence, to check for convergence, I am running a secondary controller to collect “ground truth” data of what a controller will do at this terminal state.
I don’t think the multi-phase OCP will work if it is affected by the second phase though since the first controller shouldn’t have information beyond it’s short horizon. Let me know if my understanding of it is wrong.
Ok I see. Which solver status do you get if the second solver fails?
In general, you might want to play around with the globalization options and/or regularization.
It is always the min step error from the QP solver. I investigated into the terminal node’s trajectory over the timesteps,and it is relatively smooth, unlike what I hypothesized. Perhaps, it is too close to the hard constraints for obstacle avoidance, as I do not have potential fields/repulsion cost.
I do currently have merit_backtracking turned on but no regularization. From what I understand, regularization can help if there are NANs detected in the hessian but this is not my case currently.
I will perhaps try to add a repulsion cost first so that the controller will stay further from the hard constraint boundary and see if it helps but any other suggestions are greatly appreciated.
I have currently implemented a capsule to capsule nearest distance constraint and applied it to stage and terminal nodes as shown below. constr_type is left as BGH.