Robot pose not working
Fix various bug related to the z-coordinate