Locomotion and Pathfinding

Fixed issues