Fixed an error in discreteMotionValid and added a python script for visualizing RRT output