did run dual arm ik ocp, but there's something evil there. removed final velocity to be 0 because it makes no sense for an mpc, did put a comment to make it optional if there is mpc. wrote what we have and what's missing for RealHeronRobotManager
