https://github.com/RoboticExplorationLab/legged_mpc_control 
https://github.com/RoboticExplorationLab/legged_mpc_control