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 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;
  }