private FramePose2d offsetMidFeetPose2d(
      FramePose2d initialPose, double walkDistance, Vector2d walkDirection) {
    FramePose2d ret = new FramePose2d(initialPose);

    walkDirection.normalize();
    ret.setX(initialPose.getX() + walkDistance * walkDirection.getX());
    ret.setY(initialPose.getY() + walkDistance * walkDirection.getY());

    return ret;
  }
  private FramePose2d getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(
      HumanoidFloatingRootJointRobot robot) {
    FramePose midFeetPose = getRobotMidFeetPose(robot);

    FramePose2d ret = new FramePose2d();
    ret.setPoseIncludingFrame(
        ReferenceFrame.getWorldFrame(),
        midFeetPose.getX(),
        midFeetPose.getY(),
        midFeetPose.getYaw());

    return ret;
  }
  private WalkToGoalBehavior testWalkToGoalBehavior(
      double walkDistance, Vector2d walkDirection, double trajectoryTime)
      throws SimulationExceededMaximumTimeException {
    FramePose2d initialMidFeetPose =
        getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(robot);

    FramePose2d desiredMidFeetPose =
        offsetMidFeetPose2d(initialMidFeetPose, walkDistance, walkDirection);

    final WalkToGoalBehavior walkToGoalBehavior =
        new WalkToGoalBehavior(
            communicationBridge,
            fullRobotModel,
            yoTime,
            getRobotModel().getPhysicalProperties().getAnkleHeight());
    communicationBridge.attachGlobalListener(
        walkToGoalBehavior.getControllerGlobalPacketConsumer());
    walkToGoalBehavior.initialize();

    WalkToGoalBehaviorPacket requestedGoal =
        new WalkToGoalBehaviorPacket(
            desiredMidFeetPose.getX(),
            desiredMidFeetPose.getY(),
            desiredMidFeetPose.getYaw(),
            RobotSide.RIGHT);
    walkToGoalBehavior.consumeObjectFromNetworkProcessor(requestedGoal);

    WalkToGoalBehaviorPacket walkToGoalFindPathPacket =
        new WalkToGoalBehaviorPacket(WalkToGoalAction.FIND_PATH);
    walkToGoalBehavior.consumeObjectFromNetworkProcessor(walkToGoalFindPathPacket);

    WalkToGoalBehaviorPacket walkToGoalExecutePacket =
        new WalkToGoalBehaviorPacket(WalkToGoalAction.EXECUTE);
    walkToGoalBehavior.consumeObjectFromNetworkProcessor(walkToGoalExecutePacket);
    walkToGoalBehavior.doControl();
    assertTrue(walkToGoalBehavior.hasInputBeenSet());

    boolean success = executeBehavior(walkToGoalBehavior, trajectoryTime);
    assertTrue(success);

    FramePose2d finalMidFeetPose =
        getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(robot);
    //      FramePose2d finalMidFeetPose = getCurrentMidFeetPose2d(referenceFrames);

    PrintTools.debug(this, " initial Midfeet Pose :\n" + initialMidFeetPose + "\n");
    assertPosesAreWithinThresholds(desiredMidFeetPose, finalMidFeetPose);

    return walkToGoalBehavior;
  }
  private void assertPosesAreWithinThresholds(FramePose2d framePose1, FramePose2d framePose2) {
    double positionDistance = framePose1.getPositionDistance(framePose2);
    double orientationDistance = framePose1.getOrientationDistance(framePose2);

    if (DEBUG) {
      PrintTools.debug(this, " desired Midfeet Pose :\n" + framePose1 + "\n");
      PrintTools.debug(this, " actual Midfeet Pose :\n" + framePose2 + "\n");

      PrintTools.debug(this, " positionDistance = " + positionDistance);
      PrintTools.debug(this, " orientationDistance = " + orientationDistance);
    }

    assertEquals(0.0, positionDistance, POSITION_THRESHOLD);
    assertEquals(0.0, orientationDistance, ORIENTATION_THRESHOLD);
  }
  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;
  }