added work-in-progress colored prm implementation based on the ompl prm