public void setListenToHighLevelStatePackets(boolean isListening) {
   if (highLevelHumanoidControllerManager != null)
     highLevelHumanoidControllerManager.setListenToHighLevelStatePackets(isListening);
   else isListeningToHighLevelStatePackets = isListening;
 }
  public RobotController getController(
      FullHumanoidRobotModel fullRobotModel,
      double controlDT,
      double gravity,
      DoubleYoVariable yoTime,
      YoGraphicsListRegistry yoGraphicsListRegistry,
      ForceSensorDataHolderReadOnly forceSensorDataHolder,
      CenterOfMassDataHolderReadOnly centerOfMassDataHolder,
      ContactSensorHolder contactSensorHolder,
      CenterOfPressureDataHolder centerOfPressureDataHolderForEstimator,
      InverseDynamicsJoint... jointsToIgnore) {
    HumanoidReferenceFrames referenceFrames = new HumanoidReferenceFrames(fullRobotModel);
    TwistCalculator twistCalculator =
        new TwistCalculator(ReferenceFrame.getWorldFrame(), fullRobotModel.getElevator());

    SideDependentList<ContactableFoot> feet =
        contactableBodiesFactory.createFootContactableBodies(fullRobotModel, referenceFrames);

    double gravityZ = Math.abs(gravity);
    double totalMass = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator());
    double totalRobotWeight = totalMass * gravityZ;

    SideDependentList<FootSwitchInterface> footSwitches =
        createFootSwitches(
            feet,
            forceSensorDataHolder,
            contactSensorHolder,
            totalRobotWeight,
            yoGraphicsListRegistry,
            registry);
    SideDependentList<ForceSensorDataReadOnly> wristForceSensors =
        createWristForceSensors(forceSensorDataHolder);

    /////////////////////////////////////////////////////////////////////////////////////////////
    // Setup the different ContactablePlaneBodies ///////////////////////////////////////////////

    RigidBody rootBody = fullRobotModel.getRootJoint().getSuccessor();
    SideDependentList<ContactablePlaneBody> handContactableBodies =
        contactableBodiesFactory.createHandContactableBodies(rootBody);

    /////////////////////////////////////////////////////////////////////////////////////////////
    // Setup the MomentumBasedController ////////////////////////////////////////////////////////
    GeometricJacobianHolder geometricJacobianHolder = new GeometricJacobianHolder();
    MomentumOptimizationSettings momentumOptimizationSettings =
        walkingControllerParameters.getMomentumOptimizationSettings();
    double omega0 = walkingControllerParameters.getOmega0();
    momentumBasedController =
        new HighLevelHumanoidControllerToolbox(
            fullRobotModel,
            geometricJacobianHolder,
            referenceFrames,
            footSwitches,
            centerOfMassDataHolder,
            wristForceSensors,
            yoTime,
            gravityZ,
            omega0,
            twistCalculator,
            feet,
            handContactableBodies,
            controlDT,
            updatables,
            yoGraphicsListRegistry,
            jointsToIgnore);
    momentumBasedController.attachControllerStateChangedListeners(
        controllerStateChangedListenersToAttach);
    attachControllerFailureListeners(controllerFailureListenersToAttach);
    if (createComponentBasedFootstepDataMessageGenerator)
      createComponentBasedFootstepDataMessageGenerator(
          useHeadingAndVelocityScript, heightMapForFootstepZ);
    if (createUserDesiredControllerCommandGenerator)
      if (createQueuedControllerCommandGenerator)
        createQueuedControllerCommandGenerator(controllerCommands);
    if (createUserDesiredControllerCommandGenerator) createUserDesiredControllerCommandGenerator();

    managerFactory.setMomentumBasedController(momentumBasedController);

    /////////////////////////////////////////////////////////////////////////////////////////////
    // Setup the WalkingHighLevelHumanoidController /////////////////////////////////////////////
    walkingBehavior =
        new WalkingHighLevelHumanoidController(
            commandInputManager,
            statusOutputManager,
            managerFactory,
            walkingControllerParameters,
            momentumBasedController);
    highLevelBehaviors.add(walkingBehavior);

    /////////////////////////////////////////////////////////////////////////////////////////////
    // Setup the DoNothingController ////////////////////////////////////////////////////////////
    // Useful as a transition state on the real robot
    DoNothingBehavior doNothingBehavior = new DoNothingBehavior(momentumBasedController);
    highLevelBehaviors.add(doNothingBehavior);

    /////////////////////////////////////////////////////////////////////////////////////////////
    // Setup the WholeBodyInverseDynamicsControlCore ////////////////////////////////////////////
    RigidBody[] controlledBodies = {
      fullRobotModel.getPelvis(),
      fullRobotModel.getFoot(RobotSide.LEFT),
      fullRobotModel.getFoot(RobotSide.RIGHT)
    };
    InverseDynamicsJoint[] jointsToOptimizeFor =
        HighLevelHumanoidControllerToolbox.computeJointsToOptimizeFor(
            fullRobotModel, jointsToIgnore);
    List<? extends ContactablePlaneBody> contactablePlaneBodies =
        momentumBasedController.getContactablePlaneBodyList();
    WholeBodyControlCoreToolbox toolbox =
        new WholeBodyControlCoreToolbox(
            fullRobotModel,
            controlledBodies,
            jointsToOptimizeFor,
            momentumOptimizationSettings,
            referenceFrames,
            controlDT,
            gravityZ,
            geometricJacobianHolder,
            twistCalculator,
            contactablePlaneBodies,
            yoGraphicsListRegistry,
            registry);
    FeedbackControlCommandList template = managerFactory.createFeedbackControlTemplate();
    WholeBodyControllerCore controllerCore =
        new WholeBodyControllerCore(toolbox, template, registry);
    ControllerCoreOutputReadOnly controllerCoreOutput =
        controllerCore.getOutputForHighLevelController();

    /////////////////////////////////////////////////////////////////////////////////////////////
    // Setup the HighLevelHumanoidControllerManager /////////////////////////////////////////////
    // This is the "highest level" controller that enables switching between
    // the different controllers (walking, multi-contact, driving, etc.)
    highLevelHumanoidControllerManager =
        new HighLevelHumanoidControllerManager(
            commandInputManager,
            statusOutputManager,
            controllerCore,
            initialBehavior,
            highLevelBehaviors,
            momentumBasedController,
            centerOfPressureDataHolderForEstimator,
            controllerCoreOutput);
    highLevelHumanoidControllerManager.setFallbackControllerForFailure(
        HighLevelState.DO_NOTHING_BEHAVIOR);
    highLevelHumanoidControllerManager.addYoVariableRegistry(registry);
    highLevelHumanoidControllerManager.setListenToHighLevelStatePackets(
        isListeningToHighLevelStatePackets);

    createRegisteredControllers();

    return highLevelHumanoidControllerManager;
  }