Updated RRT implementation to support N robots for lifted planning