public void reinitializeWalking(boolean keepPosition) { highLevelHumanoidControllerManager.requestHighLevelState(HighLevelState.WALKING); if (keepPosition) { if (walkingBehavior != null) { walkingBehavior.initializeDesiredHeightToCurrent(); walkingBehavior.reinitializePelvisOrientation(false); } if (managerFactory != null) { managerFactory.getOrCreateManipulationControlModule().initializeDesiredToCurrent(); managerFactory.getOrCreatePelvisOrientationManager().setToHoldCurrentInWorldFrame(); } } }
public StandingState(CommandInputManager commandInputManager, WalkingMessageHandler walkingMessageHandler, HighLevelHumanoidControllerToolbox momentumBasedController, HighLevelControlManagerFactory managerFactory, WalkingFailureDetectionControlModule failureDetectionControlModule, WalkingControllerParameters walkingControllerParameters, YoVariableRegistry parentRegistry) { super(WalkingStateEnum.STANDING, parentRegistry); this.commandInputManager = commandInputManager; this.walkingMessageHandler = walkingMessageHandler; this.momentumBasedController = momentumBasedController; this.failureDetectionControlModule = failureDetectionControlModule; comHeightManager = managerFactory.getOrCreateCenterOfMassHeightManager(); balanceManager = managerFactory.getOrCreateBalanceManager(); manipulationControlModule = managerFactory.getOrCreateManipulationControlModule(); pelvisOrientationManager = managerFactory.getOrCreatePelvisOrientationManager(); doPrepareManipulationForLocomotion.set(walkingControllerParameters.doPrepareManipulationForLocomotion()); doPreparePelvisForLocomotion.set(walkingControllerParameters.doPreparePelvisForLocomotion()); }
public WalkingCommandConsumer(CommandInputManager commandInputManager, StatusMessageOutputManager statusMessageOutputManager, HighLevelHumanoidControllerToolbox momentumBasedController, WalkingMessageHandler walkingMessageHandler, HighLevelControlManagerFactory managerFactory, WalkingControllerParameters walkingControllerParameters, YoVariableRegistry parentRegistry) { this.walkingMessageHandler = walkingMessageHandler; yoTime = momentumBasedController.getYoTime(); this.commandInputManager = commandInputManager; this.statusMessageOutputManager = statusMessageOutputManager; pelvisOrientationManager = managerFactory.getOrCreatePelvisOrientationManager(); chestOrientationManager = managerFactory.getOrCreateChestOrientationManager(); headOrientationManager = managerFactory.getOrCreatedHeadOrientationManager(); manipulationControlModule = managerFactory.getOrCreateManipulationControlModule(); feetManager = managerFactory.getOrCreateFeetManager(); balanceManager = managerFactory.getOrCreateBalanceManager(); comHeightManager = managerFactory.getOrCreateCenterOfMassHeightManager(); isAutomaticManipulationAbortEnabled.set(walkingControllerParameters.allowAutomaticManipulationAbort()); icpErrorThresholdToAbortManipulation.set(0.04); minimumDurationBetweenTwoManipulationAborts.set(5.0); manipulationIgnoreInputsDurationAfterAbort.set(2.0); timeOfLastManipulationAbortRequest.set(Double.NEGATIVE_INFINITY); parentRegistry.addChild(registry); }
this.headOrientationManager = managerFactory.getOrCreatedHeadOrientationManager(); this.chestOrientationManager = managerFactory.getOrCreateChestOrientationManager(); this.manipulationControlModule = managerFactory.getOrCreateManipulationControlModule(); this.feetManager = managerFactory.getOrCreateFeetManager();