Planner mods

Changed the code in SimBA to reflect the relocation of the Protobufs and URDFParser to the HAL library.
ALso, `eigen::Matrix4d::inverse()` is a builtin for this.