add collision free constraint to the getSuccessor function, haven't validate yet