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;
}
}