public void createQueuedControllerCommandGenerator(
ConcurrentLinkedQueue<Command<?, ?>> controllerCommands) {
if (queuedControllerCommandGenerator != null) return;
if (momentumBasedController != null) {
System.out.println("In createdQueuedControllerCommandGenerator");
SideDependentList<ContactableFoot> contactableFeet =
momentumBasedController.getContactableFeet();
CommonHumanoidReferenceFrames referenceFrames = momentumBasedController.getReferenceFrames();
double controlDT = momentumBasedController.getControlDT();
queuedControllerCommandGenerator =
new QueuedControllerCommandGenerator(
controllerCommands,
commandInputManager,
statusOutputManager,
walkingControllerParameters,
referenceFrames,
contactableFeet,
controlDT,
useHeadingAndVelocityScript,
registry);
momentumBasedController.addUpdatables(queuedControllerCommandGenerator.getModulesToUpdate());
} else {
createQueuedControllerCommandGenerator = true;
this.controllerCommands = controllerCommands;
}
}
public void createComponentBasedFootstepDataMessageGenerator(
boolean useHeadingAndVelocityScript, HeightMap heightMapForFootstepZ) {
if (footstepGenerator != null) return;
if (momentumBasedController != null) {
SideDependentList<ContactableFoot> contactableFeet =
momentumBasedController.getContactableFeet();
CommonHumanoidReferenceFrames referenceFrames = momentumBasedController.getReferenceFrames();
double controlDT = momentumBasedController.getControlDT();
ComponentBasedFootstepDataMessageGenerator footstepGenerator =
new ComponentBasedFootstepDataMessageGenerator(
commandInputManager,
statusOutputManager,
walkingControllerParameters,
referenceFrames,
contactableFeet,
controlDT,
useHeadingAndVelocityScript,
heightMapForFootstepZ,
registry);
momentumBasedController.addUpdatables(footstepGenerator.getModulesToUpdate());
} else {
createComponentBasedFootstepDataMessageGenerator = true;
this.useHeadingAndVelocityScript = useHeadingAndVelocityScript;
this.heightMapForFootstepZ = heightMapForFootstepZ;
}
}
public void createUserDesiredControllerCommandGenerator() {
if (userDesiredControllerCommandGenerators != null) return;
if (momentumBasedController != null) {
double defaultTrajectoryTime = 1.0;
SideDependentList<ContactableFoot> contactableFeet =
momentumBasedController.getContactableFeet();
userDesiredControllerCommandGenerators =
new UserDesiredControllerCommandGenerators(
commandInputManager,
momentumBasedController.getFullRobotModel(),
momentumBasedController.getReferenceFrames(),
contactableFeet,
walkingControllerParameters,
defaultTrajectoryTime,
registry);
} else {
createUserDesiredControllerCommandGenerator = true;
}
}
public void attachRobotMotionStatusChangedListener(RobotMotionStatusChangedListener listener) {
momentumBasedController.attachRobotMotionStatusChangedListener(listener);
}
public void attachControllerStateChangedListener(ControllerStateChangedListener listener) {
if (momentumBasedController != null)
momentumBasedController.attachControllerStateChangedListener(listener);
else controllerStateChangedListenersToAttach.add(listener);
}
public void attachControllerFailureListener(ControllerFailureListener listener) {
if (momentumBasedController != null)
momentumBasedController.attachControllerFailureListener(listener);
else controllerFailureListenersToAttach.add(listener);
}
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;
}
public void setInverseDynamicsCalculatorListener(
InverseDynamicsCalculatorListener inverseDynamicsCalculatorListener) {
momentumBasedController.setInverseDynamicsCalculatorListener(inverseDynamicsCalculatorListener);
}