private FramePose2d getCurrentMidFeetPose2d(HumanoidReferenceFrames referenceFrames) {
robotDataReceiver.updateRobotModel();
referenceFrames.updateFrames();
ReferenceFrame midFeetFrame = referenceFrames.getMidFeetZUpFrame();
FramePose midFeetPose = new FramePose();
midFeetPose.setToZero(midFeetFrame);
midFeetPose.changeFrame(ReferenceFrame.getWorldFrame());
FramePose2d ret = new FramePose2d();
ret.setPoseIncludingFrame(
midFeetPose.getReferenceFrame(),
midFeetPose.getX(),
midFeetPose.getY(),
midFeetPose.getYaw());
return ret;
}
