Hi,
I’m porting code that was written for the ACADO toolkit. I came across the function acado_preparationStep() that apparently must be called to initialize their solver, i.e. before acado_feedbackStep(). This function is important enough that some implementations spawn dedicated threads to run it.
How do these concepts map to acados and its {{ model.name }}_acados_solve() function? Does the latter function carries out preparation and solving in one fell swoop?
Hi,
in acados, the solve() function does not require a preparation by default.
It carries out the full SQP[-RTI] solve.
However, the acados SQP-RTI method does allow to split preparation and feedback phase. By setting rti_phase.
Here, rti_phase = 0; means full (preparation + feedback)
rti_phase = 1; means only preparation step
rti_phase = 2; means only feedback step
Best,
Jonathan
Great! Thank you. It does seem to me that fusing preparation and solve makes for a more intuitive interface.
For the sake of documenting the differences with ACADO, can I emulate acado_preparationStep() by
int preparation = 1;
ocp_nlp_solver_opts_set(cfg, opt, "rti_phase", &preparation);
MyModel_solve(capsule);
and likewise emulate acado_feedbackStep() by setting rti_phase to 2 then calling solve()?
By the way, do you make use of OpenMP parallelization to speed up the preparation phase and obviate the need for separate preparation?
hello FreyJo !
my prevous python RTI code 
for i in range(N_sim):
# preparation phase
ocp_solver.options_set('rti_phase', 1)
status = ocp_solver.solve()
t_preparation[i] = ocp_solver.get_stats('time_tot')
ocp_solver.set(0, "ubx", simX[i, :])
ocp_solver.set(0, "lbx", simX[i, :])
ocp_solver.options_set('rti_phase', 2)
status = ocp_solver.solve()
t_feedback[i] = ocp_solver.get_stats('time_tot')
simU[i, :] = ocp_solver.get(0, "u")
# simU[i,:] = ocp_solver.solve_for_x0(x0_bar = simX[i, :])
simX[i+1, :] = integrator.simulate(x=simX[i, :], u=simU[i,:])
i change the generated_code to this RTI step. is this right?
// RTI section
for (int ii = 0; ii < NTIMINGS; ii++) {
// ocp_nlp_solver_opts_set(nlp_config, "rti_phase", 1);
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &RTI_phase_prepare);
int status = quad_body_3d_acados_solve(acados_ocp_capsule);
ocp_nlp_get(nlp_solver, "time_tot", &elapsed_time);
min_time = MIN(elapsed_time, min_time);
printf("\n--- rti_phase %d ---\n", ii);
// get current state, set the lbx ubx and solve
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "lbx", x_init);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "ubx", x_init);
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &RTI_phase_feed);
status = quad_body_3d_acados_solve(acados_ocp_capsule);
// ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, 0, "x", x_init)
// ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", u0);
}
Hi 
some remarks on your code:
time_preparation and time_feedback. I suggest you use those.ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); and ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);Best, Katrin
Much thanks to you kaethe !
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0); and ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
so i change the lbx0 and ubx0 to the the initial state?
another question is :
yes exactly lbx0 and ubx0 should be set to your initial state.
You will need to update y_ref in the preparation phase. Updating x0 can be done in the feedback phase.
Thanks to you kaethe! I will soon try it !
hello kaethe! In prevous acado framework, i can set the reference traj , and do not prepare to be faster. Only do preparation in the first scratch and after feedback.So in a very little period , it can work? I know that setting the y_ref should be in the preparation to get the objects to min, but in the real robotics system, the y_ref can change in a very small time, update y_ref and prepare can consume much time.
so can i do not prepare after setting the y_ref, and do preparation only in the first solve and after the feedback?
template<typename T>
quadrotor_common::ControlCommand MpcController<T>::run(
const quadrotor_common::QuadStateEstimate& state_estimate,
const quadrotor_common::Trajectory& reference_trajectory,
const MpcParams<T>& params) {
ros::Time call_time = ros::Time::now();
const clock_t start = clock();
if (params.changed_) {
params_ = params;
setNewParams(params_);
}
preparation_thread_.join();
// Convert everything into Eigen format.
setStateEstimate(state_estimate);
setReference(reference_trajectory);
static const bool do_preparation_step(false);
// Get the feedback from MPC.
mpc_wrapper_.setTrajectory(reference_states_, reference_inputs_);
if (solve_from_scratch_) {
ROS_INFO("Solving MPC with hover as initial guess.");
mpc_wrapper_.solve(est_state_);
solve_from_scratch_ = false;
} else {
mpc_wrapper_.update(est_state_, do_preparation_step);
}
mpc_wrapper_.getStates(predicted_states_);
mpc_wrapper_.getInputs(predicted_inputs_);
Not sure if I understand your question. Conceptually, the order should be as follows:
# preparation phase
update_reference(new_reference)
set("rti_phase", 1)
solve()
# feedback phase
update_initial_state(new_initial_state)
set("rti_phase", 2)
solve()
thanks to you ! my previous acado order is this :
if first solve:
update_reference(now_state)
update_initial_state(now_state)
set("rti_phase", 1)
solve()
update_initial_state(new_initial_state)
set("rti_phase", 2)
solve()
else:
update_reference(new_reference)
# feedback phase
update_initial_state(new_initial_state)
set("rti_phase", 2)
solve()
# prepare phase
set("rti_phase", 1)
solve()
The if part seems reasonable, the else part looks weird to me. Why would you want to do something else at all?
The “prepare” step is quite time-consuming. Therefore, I intend to perform “feedback” first whenever I control the loop. This is very quick. Then, I’ll open a separate thread for “prepare” so that it’s ready for the next “feedback”.
This logic doesn’t apply to the first solution, because “prepare” must be done before each “feedback”. So I’ve divided it into two logics with “if” and “else”. In this way, when I implement robot control, the control delay is basically just the time taken by “feedback”. However, if every time after getting the measurement data or the reference trajectory, we first do “prepare” and then “feedback” within one program, the time consumption will be the sum of the time for “prepare” and “feedback”.