Added basic collision detection to moveit.py