public void createQueuedControllerCommandGenerator(
      ConcurrentLinkedQueue<Command<?, ?>> controllerCommands) {
    if (queuedControllerCommandGenerator != null) return;

    if (momentumBasedController != null) {
      System.out.println("In createdQueuedControllerCommandGenerator");

      SideDependentList<ContactableFoot> contactableFeet =
          momentumBasedController.getContactableFeet();
      CommonHumanoidReferenceFrames referenceFrames = momentumBasedController.getReferenceFrames();
      double controlDT = momentumBasedController.getControlDT();
      queuedControllerCommandGenerator =
          new QueuedControllerCommandGenerator(
              controllerCommands,
              commandInputManager,
              statusOutputManager,
              walkingControllerParameters,
              referenceFrames,
              contactableFeet,
              controlDT,
              useHeadingAndVelocityScript,
              registry);

      momentumBasedController.addUpdatables(queuedControllerCommandGenerator.getModulesToUpdate());
    } else {
      createQueuedControllerCommandGenerator = true;
      this.controllerCommands = controllerCommands;
    }
  }
  public void createComponentBasedFootstepDataMessageGenerator(
      boolean useHeadingAndVelocityScript, HeightMap heightMapForFootstepZ) {
    if (footstepGenerator != null) return;

    if (momentumBasedController != null) {
      SideDependentList<ContactableFoot> contactableFeet =
          momentumBasedController.getContactableFeet();
      CommonHumanoidReferenceFrames referenceFrames = momentumBasedController.getReferenceFrames();
      double controlDT = momentumBasedController.getControlDT();
      ComponentBasedFootstepDataMessageGenerator footstepGenerator =
          new ComponentBasedFootstepDataMessageGenerator(
              commandInputManager,
              statusOutputManager,
              walkingControllerParameters,
              referenceFrames,
              contactableFeet,
              controlDT,
              useHeadingAndVelocityScript,
              heightMapForFootstepZ,
              registry);
      momentumBasedController.addUpdatables(footstepGenerator.getModulesToUpdate());
    } else {
      createComponentBasedFootstepDataMessageGenerator = true;
      this.useHeadingAndVelocityScript = useHeadingAndVelocityScript;
      this.heightMapForFootstepZ = heightMapForFootstepZ;
    }
  }