Added ideal_theta_k
Added hybrid control attempt