Add: graph construction for PRM with adjacency matrix. No avoiding trajectory through obstacles yet