Planner mods

Changed the code in SimBA to reflect the relocation of the Protobufs and URDFParser to the HAL library.
I believe this could simplify to `return btVector3(v(0), v(1), v(2))`