Efficient Numerical Optimal Control For Motion Planning And Control of Mobile Robots