private void createRegisteredControllers() {
for (int i = 0; i < highLevelBehaviorFactories.size(); i++) {
HighLevelBehaviorFactory highLevelBehaviorFactory = highLevelBehaviorFactories.get(i);
HighLevelBehavior highLevelBehavior =
highLevelBehaviorFactory.createHighLevelBehavior(managerFactory, momentumBasedController);
boolean transitionRequested = highLevelBehaviorFactory.isTransitionToBehaviorRequested();
highLevelHumanoidControllerManager.addHighLevelBehavior(
highLevelBehavior, transitionRequested);
}
}
public void addHighLevelBehaviorFactory(HighLevelBehaviorFactory highLevelBehaviorFactory) {
if (momentumBasedController == null) {
highLevelBehaviorFactories.add(highLevelBehaviorFactory);
} else {
HighLevelBehavior highLevelBehavior =
highLevelBehaviorFactory.createHighLevelBehavior(managerFactory, momentumBasedController);
boolean transitionToBehaviorRequested =
highLevelBehaviorFactory.isTransitionToBehaviorRequested();
highLevelHumanoidControllerManager.addHighLevelBehavior(
highLevelBehavior, transitionToBehaviorRequested);
}
}
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 HighLevelState getCurrentHighLevelState() {
return highLevelHumanoidControllerManager.getCurrentHighLevelState();
}
public void setFallbackControllerForFailure(HighLevelState fallbackController) {
highLevelHumanoidControllerManager.setFallbackControllerForFailure(fallbackController);
}
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;
}
