Added path planning to SimpleSim.
Added path planning
Revert "Added path planning"

This reverts commit 8c6cdb83a9902be27471d8384fcff29c2b84bdc5.
Revert "Added path planning to SimpleSim."

This reverts commit 48f71744a3985e2509f9df4fdbee94612bbe2bc3.
Added path planning to SimpleSim.