leggedrobotics / ocs2 C++ 1.5K 330 Optimal Control for Switched Systems optimal-control mpc robotics control