public MomentumBasedControllerFactory(ContactableBodiesFactory contactableBodiesFactory, SideDependentList<String> footForceSensorNames, SideDependentList<String> footContactSensorNames, SideDependentList<String> wristSensorNames, WalkingControllerParameters walkingControllerParameters, ArmControllerParameters armControllerParameters, CapturePointPlannerParameters capturePointPlannerParameters, HighLevelState initialBehavior) { this.footSensorNames = footForceSensorNames; this.footContactSensorNames = footContactSensorNames; this.wristSensorNames = wristSensorNames; this.contactableBodiesFactory = contactableBodiesFactory; this.initialBehavior = initialBehavior; this.walkingControllerParameters = walkingControllerParameters; this.capturePointPlannerParameters = capturePointPlannerParameters; commandInputManager = new CommandInputManager(ControllerAPIDefinition.getControllerSupportedCommands()); statusOutputManager = new StatusMessageOutputManager(ControllerAPIDefinition.getControllerSupportedStatusMessages()); managerFactory = new HighLevelControlManagerFactory(statusOutputManager, registry); managerFactory.setArmControlParameters(armControllerParameters); managerFactory.setCapturePointPlannerParameters(capturePointPlannerParameters); managerFactory.setWalkingControllerParameters(walkingControllerParameters); }
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); }
public TransferState(RobotSide transferToSide, WalkingStateEnum transferStateEnum, WalkingMessageHandler walkingMessageHandler, HighLevelHumanoidControllerToolbox momentumBasedController, HighLevelControlManagerFactory managerFactory, WalkingFailureDetectionControlModule failureDetectionControlModule, YoVariableRegistry parentRegistry) { super(transferStateEnum, parentRegistry); this.transferToSide = transferToSide; this.walkingMessageHandler = walkingMessageHandler; this.failureDetectionControlModule = failureDetectionControlModule; this.momentumBasedController = momentumBasedController; comHeightManager = managerFactory.getOrCreateCenterOfMassHeightManager(); balanceManager = managerFactory.getOrCreateBalanceManager(); pelvisOrientationManager = managerFactory.getOrCreatePelvisOrientationManager(); feetManager = managerFactory.getOrCreateFeetManager(); }
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()); }
controllerToolbox.setWalkingMessageHandler(walkingMessageHandler); managerFactory = new HighLevelControlManagerFactory(statusOutputManager, managerFactoryParent); managerFactory.setHighLevelHumanoidControllerToolbox(controllerToolbox); managerFactory.setWalkingControllerParameters(walkingControllerParameters); managerFactory.setCapturePointPlannerParameters(capturePointPlannerParameters);
public WalkingSingleSupportState(RobotSide supportSide, WalkingMessageHandler walkingMessageHandler, HighLevelHumanoidControllerToolbox momentumBasedController, HighLevelControlManagerFactory managerFactory, WalkingControllerParameters walkingControllerParameters, WalkingFailureDetectionControlModule failureDetectionControlModule, YoVariableRegistry parentRegistry) { super(supportSide, WalkingStateEnum.getWalkingSingleSupportState(supportSide), walkingMessageHandler, momentumBasedController, managerFactory, parentRegistry); this.momentumBasedController = momentumBasedController; this.failureDetectionControlModule = failureDetectionControlModule; comHeightManager = managerFactory.getOrCreateCenterOfMassHeightManager(); pelvisOrientationManager = managerFactory.getOrCreatePelvisOrientationManager(); feetManager = managerFactory.getOrCreateFeetManager(); icpErrorThresholdToSpeedUpSwing.set(walkingControllerParameters.getICPErrorThresholdToSpeedUpSwing()); finishSingleSupportWhenICPPlannerIsDone.set(walkingControllerParameters.finishSingleSupportWhenICPPlannerIsDone()); minimizeAngularMomentumRateZDuringSwing.set(walkingControllerParameters.minimizeAngularMomentumRateZDuringSwing()); }
createUserDesiredControllerCommandGenerator(); managerFactory.setMomentumBasedController(momentumBasedController); jointPrivilegedConfigurationParameters, referenceFrames, controlDT, gravityZ, geometricJacobianHolder, twistCalculator, contactablePlaneBodies, yoGraphicsListRegistry, registry); FeedbackControlCommandList template = managerFactory.createFeedbackControlTemplate(); WholeBodyControllerCore controllerCore = new WholeBodyControllerCore(toolbox, template, registry); ControllerCoreOutputReadOnly controllerCoreOutput = controllerCore.getOutputForHighLevelController();
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(); } } }
private void createControllerCore(YoVariableRegistry walkingControllerRegistry) { JointBasics[] jointsToIgnore = DRCControllerThread.createListOfJointsToIgnore(fullRobotModel, robotModel, robotModel.getSensorInformation()); JointBasics[] jointsToOptimizeFor = HighLevelHumanoidControllerToolbox.computeJointsToOptimizeFor(fullRobotModel, jointsToIgnore); FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); ReferenceFrame centerOfMassFrame = referenceFrames.getCenterOfMassFrame(); WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters(); MomentumOptimizationSettings momentumOptimizationSettings = walkingControllerParameters.getMomentumOptimizationSettings(); WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, gravityZ, rootJoint, jointsToOptimizeFor, centerOfMassFrame, momentumOptimizationSettings, yoGraphicsListRegistry, walkingControllerRegistry); toolbox.setupForInverseDynamicsSolver(contactableBodies); JointPrivilegedConfigurationParameters jointPrivilegedConfigurationParameters = walkingControllerParameters.getJointPrivilegedConfigurationParameters(); toolbox.setJointPrivilegedConfigurationParameters(jointPrivilegedConfigurationParameters); FeedbackControlCommandList template = managerFactory.createFeedbackControlTemplate(); controllerOutput = new JointDesiredOutputList(fullRobotModel.getControllableOneDoFJoints()); controllerCore = new WholeBodyControllerCore(toolbox, template, controllerOutput, walkingControllerRegistry); }
public SingleSupportState(RobotSide supportSide, WalkingStateEnum singleSupportStateEnum, WalkingMessageHandler walkingMessageHandler, HighLevelHumanoidControllerToolbox momentumBasedController, HighLevelControlManagerFactory managerFactory, YoVariableRegistry parentRegistry) { super(singleSupportStateEnum, parentRegistry); this.supportSide = supportSide; swingSide = supportSide.getOppositeSide(); minimumSwingFraction.set(0.5); this.walkingMessageHandler = walkingMessageHandler; footSwitches = momentumBasedController.getFootSwitches(); fullRobotModel = momentumBasedController.getFullRobotModel(); balanceManager = managerFactory.getOrCreateBalanceManager(); }
public TransferToStandingState(WalkingMessageHandler walkingMessageHandler, HighLevelHumanoidControllerToolbox momentumBasedController, HighLevelControlManagerFactory managerFactory, WalkingFailureDetectionControlModule failureDetectionControlModule, YoVariableRegistry parentRegistry) { super(WalkingStateEnum.TO_STANDING, parentRegistry); maxICPErrorToSwitchToStanding.set(0.025); this.walkingMessageHandler = walkingMessageHandler; this.momentumBasedController = momentumBasedController; this.failureDetectionControlModule = failureDetectionControlModule; comHeightManager = managerFactory.getOrCreateCenterOfMassHeightManager(); balanceManager = managerFactory.getOrCreateBalanceManager(); pelvisOrientationManager = managerFactory.getOrCreatePelvisOrientationManager(); feetManager = managerFactory.getOrCreateFeetManager(); doFootExplorationInTransferToStanding.set(false); }
public FlamingoStanceState(RobotSide supportSide, WalkingMessageHandler walkingMessageHandler, HighLevelHumanoidControllerToolbox momentumBasedController, HighLevelControlManagerFactory managerFactory, WalkingFailureDetectionControlModule failureDetectionControlModule, YoVariableRegistry parentRegistry) { super(supportSide, WalkingStateEnum.getFlamingoSingleSupportState(supportSide), walkingMessageHandler, momentumBasedController, managerFactory, parentRegistry); bipedSupportPolygons = momentumBasedController.getBipedSupportPolygons(); this.failureDetectionControlModule = failureDetectionControlModule; comHeightManager = managerFactory.getOrCreateCenterOfMassHeightManager(); pelvisOrientationManager = managerFactory.getOrCreatePelvisOrientationManager(); feetManager = managerFactory.getOrCreateFeetManager(); String namePrefix = supportSide.getOppositeSide().getLowerCaseName(); loadFoot = new BooleanYoVariable(namePrefix + "LoadFoot", registry); loadFootStartTime = new DoubleYoVariable(namePrefix + "LoadFootStartTime", registry); loadFootDuration = new DoubleYoVariable(namePrefix + "LoadFootDuration", registry); loadFootTransferDuration = new DoubleYoVariable(namePrefix + "LoadFootTransferDuration", registry); loadFoot.set(false); loadFootDuration.set(1.2); loadFootTransferDuration.set(0.8); }
this.pelvisOrientationManager = managerFactory.getOrCreatePelvisOrientationManager(); this.headOrientationManager = managerFactory.getOrCreatedHeadOrientationManager(); this.chestOrientationManager = managerFactory.getOrCreateChestOrientationManager(); this.manipulationControlModule = managerFactory.getOrCreateManipulationControlModule(); this.feetManager = managerFactory.getOrCreateFeetManager(); balanceManager = managerFactory.getOrCreateBalanceManager(); comHeightManager = managerFactory.getOrCreateCenterOfMassHeightManager();