Initial version of Whisker Wallfollowing