forward-kinematics inverse-kinematics library model-predictive-control nonlinear optimal-control optimization planning python robotics task-specification trajectory-optimization