Corrected some bugs and I think I found the issue with the results: the gain k_1 is always set to one. This shouldn't happen, since it's meant to saturate the control torques.