public void setupHighLevelStates(HighLevelHumanoidControllerFactory controllerFactory, SideDependentList<String> feetForceSensorNames, HighLevelControllerName fallbackControllerState) { controllerFactory.useDefaultDoNothingControlState(); controllerFactory.useDefaultWalkingControlState(); controllerFactory.addRequestableTransition(DO_NOTHING_BEHAVIOR, WALKING); controllerFactory.addRequestableTransition(WALKING, DO_NOTHING_BEHAVIOR); controllerFactory.addControllerFailureTransition(DO_NOTHING_BEHAVIOR, fallbackControllerState); controllerFactory.addControllerFailureTransition(WALKING, fallbackControllerState); controllerFactory.setInitialState(HighLevelControllerName.WALKING); }
public void attachControllerFailureListener(ControllerFailureListener listener) { if (controllerFactory == null) this.controllerFailureListeners.add(listener); else controllerFactory.attachControllerFailureListener(listener); }
public void setupHighLevelStates(HighLevelHumanoidControllerFactory controllerFactory) { controllerFactory.useDefaultDoNothingControlState(); controllerFactory.useDefaultWalkingControlState(); controllerFactory.addRequestableTransition(DO_NOTHING_BEHAVIOR, WALKING); controllerFactory.addRequestableTransition(WALKING, DO_NOTHING_BEHAVIOR); }
SideDependentList<String> feetForceSensorNames = sensorInformation.getFeetForceSensorNames(); SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); HighLevelHumanoidControllerFactory controllerFactory = new HighLevelHumanoidControllerFactory(contactableBodiesFactory, feetForceSensorNames, feetContactSensorNames, wristForceSensorNames, highLevelControllerParameters, walkingControllerParamaters, capturePointPlannerParameters); controllerFactory.useDefaultDoNothingControlState(); controllerFactory.useDefaultWalkingControlState(); controllerFactory.addRequestableTransition(DO_NOTHING_BEHAVIOR, WALKING); controllerFactory.addRequestableTransition(WALKING, DO_NOTHING_BEHAVIOR); controllerFactory.addControllerFailureTransition(DO_NOTHING_BEHAVIOR, fallbackControllerState); controllerFactory.addControllerFailureTransition(WALKING, fallbackControllerState); controllerFactory.setInitialState(highLevelControllerParameters.getDefaultInitialControllerState()); controllerFactory.addCustomControlState(diagnosticsWhenHangingHighLevelBehaviorFactory); controllerFactory.createControllerNetworkSubscriber(new PeriodicRealtimeThreadScheduler(poseCommunicatorPriority), packetCommunicator); controllerFactory.createComponentBasedFootstepDataMessageGenerator();
SideDependentList<String> feetForceSensorNames = sensorInformation.getFeetForceSensorNames(); SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); HighLevelHumanoidControllerFactory controllerFactory = new HighLevelHumanoidControllerFactory(contactableBodiesFactory, feetForceSensorNames, feetContactSensorNames, wristForceSensorNames, highLevelControllerParameters, walkingControllerParameters, capturePointPlannerParameters); controllerFactory.createControllerNetworkSubscriber(robotModel.getSimpleRobotName(), realtimeRos2Node); controllerFactory.setInitialState(highLevelControllerParameters.getDefaultInitialControllerState()); controllerFactory.useDefaultStandPrepControlState(); controllerFactory.useDefaultStandReadyControlState(); controllerFactory.useDefaultStandTransitionControlState(); controllerFactory.useDefaultWalkingControlState(); controllerFactory.useDefaultExitWalkingTransitionControlState(STAND_PREP_STATE); controllerFactory.addCustomControlState(calibrationStateFactory); controllerFactory.addRequestableTransition(calibrationState, STAND_PREP_STATE); controllerFactory.addFinishedTransition(calibrationState, STAND_PREP_STATE); controllerFactory.addFinishedTransition(STAND_PREP_STATE, STAND_READY); controllerFactory.addRequestableTransition(STAND_PREP_STATE, calibrationState); controllerFactory.addRequestableTransition(STAND_READY, STAND_TRANSITION_STATE); if (fallbackControllerState != STAND_READY) controllerFactory.addControllerFailureTransition(STAND_READY, fallbackControllerState); controllerFactory.addRequestableTransition(STAND_READY, calibrationState); controllerFactory.addRequestableTransition(STAND_READY, STAND_PREP_STATE);
SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); HighLevelHumanoidControllerFactory controllerFactory = new HighLevelHumanoidControllerFactory(contactableBodiesFactory, footSensorNames, feetContactSensorNames, wristForceSensorNames, highLevelControllerParameters, walkingControllerParameters, capturePointPlannerParameters); controllerFactory.useDefaultDoNothingControlState(); controllerFactory.useDefaultWalkingControlState(); controllerFactory.addRequestableTransition(DO_NOTHING_BEHAVIOR, WALKING); controllerFactory.addRequestableTransition(WALKING, DO_NOTHING_BEHAVIOR); controllerFactory.addControllerFailureTransition(DO_NOTHING_BEHAVIOR, fallbackControllerState); controllerFactory.addControllerFailureTransition(WALKING, fallbackControllerState); controllerFactory.setInitialState(HighLevelControllerName.DO_NOTHING_BEHAVIOR); null); diagnosticsWhenHangingControllerStateFactory.setTransitionRequested(true); controllerFactory.addCustomControlState(diagnosticsWhenHangingControllerStateFactory);
SideDependentList<String> feetContactSensorNames = sensorInformation.getFeetContactSensorNames(); SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); HighLevelHumanoidControllerFactory controllerFactory = new HighLevelHumanoidControllerFactory(contactableBodiesFactory, feetForceSensorNames, feetContactSensorNames, wristForceSensorNames, highLevelControllerParameters, walkingControllerParameters, capturePointPlannerParameters); controllerFactory.createControllerNetworkSubscriber(robotModel.getSimpleRobotName(), realtimeRos2Node); controllerFactory.useDefaultDoNothingControlState(); controllerFactory.useDefaultWalkingControlState(); controllerFactory.addRequestableTransition(DO_NOTHING_BEHAVIOR, WALKING); controllerFactory.addRequestableTransition(WALKING, DO_NOTHING_BEHAVIOR); controllerFactory.addControllerFailureTransition(DO_NOTHING_BEHAVIOR, fallbackControllerState); controllerFactory.addControllerFailureTransition(WALKING, fallbackControllerState);
SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); controllerFactory = new HighLevelHumanoidControllerFactory(contactableBodiesFactory, feetForceSensorNames, feetContactSensorNames, wristForceSensorNames, highLevelControllerParameters, walkingControllerParameters, capturePointPlannerParameters); setupHighLevelStates(controllerFactory); controllerFactory.attachControllerFailureListeners(controllerFailureListeners); if (setupControllerNetworkSubscriber) controllerFactory.createControllerNetworkSubscriber(robotModel.getSimpleRobotName(), realtimeRos2Node); controllerFactory.addCustomControlState(highLevelControllerFactories.get(i)); for (int i = 0; i < controllerTransitionFactories.size(); i++) controllerFactory.addCustomStateTransition(controllerTransitionFactories.get(i)); controllerFactory.setInitialState(HighLevelControllerName.WALKING); controllerFactory.createQueuedControllerCommandGenerator(controllerCommands); controllerFactory.setHeadingAndVelocityEvaluationScriptParameters(walkingScriptParameters); controllerFactory.createComponentBasedFootstepDataMessageGenerator(useHeadingAndVelocityScript, scsInitialSetup.getHeightMap()); else if (addFootstepMessageGenerator) controllerFactory.createComponentBasedFootstepDataMessageGenerator(useHeadingAndVelocityScript); HighLevelHumanoidControllerToolbox highLevelHumanoidControllerToolbox = controllerFactory.getHighLevelHumanoidControllerToolbox(); FullHumanoidRobotModel fullRobotModel = highLevelHumanoidControllerToolbox.getFullRobotModel(); scriptBasedControllerCommandGenerator = new ScriptBasedControllerCommandGenerator(controllerCommands, fullRobotModel);
contactableBodiesFactory.addAdditionalContactPoint(additionalContactRigidBodyNames.get(i), additionalContactNames.get(i), additionalContactTransforms.get(i)); HighLevelHumanoidControllerFactory controllerFactory = new HighLevelHumanoidControllerFactory(contactableBodiesFactory, feetForceSensorNames, feetContactSensorNames, wristForceSensorNames, highLevelControllerParameters, walkingControllerParameters, capturePointPlannerParameters); setupHighLevelStates(controllerFactory, feetForceSensorNames, highLevelControllerParameters.getFallbackControllerState()); controllerFactory.setHeadingAndVelocityEvaluationScriptParameters(walkingScriptParameters); controllerFactory.createComponentBasedFootstepDataMessageGenerator(useVelocityAndHeadingScript, heightMapForFootstepZ);
controllerFactory.attachControllerStateChangedListener(stepprOutputWriter); controllerFactory.attachControllerFailureListener(stepprOutputWriter); DRCOutputProcessor drcOutputWriter = stepprOutputWriter;
SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); HighLevelHumanoidControllerFactory controllerFactory = new HighLevelHumanoidControllerFactory(contactableBodiesFactory, feetForceSensorNames, feetContactSensorNames, wristForceSensorNames, highLevelControllerParameters, walkingControllerParamaters, capturePointPlannerParameters); controllerFactory.useDefaultDoNothingControlState(); controllerFactory.useDefaultWalkingControlState(); controllerFactory.addRequestableTransition(DO_NOTHING_BEHAVIOR, WALKING); controllerFactory.addRequestableTransition(WALKING, DO_NOTHING_BEHAVIOR); controllerFactory.addControllerFailureTransition(DO_NOTHING_BEHAVIOR, fallbackControllerState); controllerFactory.addControllerFailureTransition(WALKING, fallbackControllerState); controllerFactory.setInitialState(highLevelControllerParameters.getDefaultInitialControllerState()); controllerFactory.addCustomControlState(diagnosticsWhenHangingHighLevelBehaviorFactory); controllerFactory.createControllerNetworkSubscriber(new PeriodicRealtimeThreadScheduler(poseCommunicatorPriority), packetCommunicator); controllerFactory.createComponentBasedFootstepDataMessageGenerator();
controllerFactory.attachControllerStateChangedListener(wandererOutputWriter); controllerFactory.attachControllerFailureListener(wandererOutputWriter); DRCOutputProcessor drcOutputWriter = wandererOutputWriter;
public void attachControllerFailureListener(ControllerFailureListener listener) { avatarSimulation.getHighLevelHumanoidControllerFactory().attachControllerFailureListener(listener); }