Move waypoint helper functions into PlanInstruction

[marip8]

We should consider moving the helper functions proposed here into the PlanInstruction class. The PlanInstruction already contains a ManipulatorInfo struct and waypoint information, so it seems intuitive that it should be able to report the local, world, and joint pose of its waypoint, given environment information (i.e. IK/FK solvers, link transform map).