Optimal control library for robot under contact sequence
