Planning on doing this in 2 PRs:

1. Update the frame system used by the motion plan solver using the information supplied by the transforms key in world state

2. Create constraints for the solver using the obstacles supplied by world state