After I had fully implemented MPC in a Jupyter Notebook using CasADi and the point mass state space model, I wanted to test the same model in the physics-based simulation environment Gazebo, using the Bullet physics engine.

To do that, I basically rewrote the Jupyter Notebook in C++, and generated a file describing the formulated optimization problem using the CasADi export feature.

I also made sure that the structure of my software is fully modular, meaning the controller runs completely separated from Gazebo and only receives state information at regular intervals, after which it processes this state and sends back a control action, more precisely torque commands for each joint of the robot. They currently communicate via a local UDP connection, which I will change to UNIX sockets in the near future.

This allows me to switch back and forth from simulation and real robot without changing anything about the controller, which was very important to me.

So after I finished the Gazebo Plugin that sends state messages via a UDP socket and implemented a basic version of the MPC in C++, I tested the controller using a simple floating torso in Gazebo and used the API to directly apply forces in the world frame for debugging purposes.

However, I quickly realized something was wrong because the torso kept "flying away" in the simulation, indicating wrong forces were being applied by the controller. 

This went on for a while, and I started logging every value I could think of to a CSV file. Then, I initially used GNU Octave to plot the CSV values, and after looking at some very conclusive plots

... I started logging both controller side and simulation side, suspecting the controller is working with too old state information. And there it was!

There was a clear difference of about 15ms between latest logged simulation state and the state the controller was using for the MPC calculations, which meant outdated forces were sent to Gazebo and the controller was "lagging behind" by about one time step. No wonder it was constantly exploding in the simulation.

After trying a few approaches of fixing the problem, I settled on simply stepping the discretized model by one time step and using that "compensated" state for further MPC calculations: See here

// Step the model one timestep and use the resulting state as the initial state for the solver. This compensates for the roughly 1 sample delay due to the solver time
P_param.block<n,1>(0, 0) = step_discrete_model(x_t, u_t, r_x_left, r_x_right, r_y_left, r_y_right, r_z_left, r_z_right); 

This resulted in the floating torso being stabilized in Gazebo, which was a great first step! I do have to mention, though, that a few other bugs were fixed while trying to find the root cause, so stuff like a few sign errors were probably also part of the cause :)