Probabilistic roadmaps

[stevenwaslander]

Convert to functions the sampling methods, investigate efficiency of sampling approaches. Move environment generation to environment and develop multiple examples with different environments.