Constrained Nonlinear Heuristic-Based MPC for Control of Robotic Systems with Uncertainty