Navigation test for icub demo