Added initial RobotMotionPlanner
Updated Messy2Controller