This produces the foward and angular velocity commands based on where the robot is and where it wants to go.