@Override public void attachControllerFailureListener(ControllerFailureListener listener) { if (controllerFactory == null) this.controllerFailureListeners.add(listener); else controllerFactory.attachControllerFailureListener(listener); }
public void createComponentBasedFootstepDataMessageGenerator() { createComponentBasedFootstepDataMessageGenerator(false, null); }
public void attachControllerStateChangedListeners(List<ControllerStateChangedListener> listeners) { for (int i = 0; i < listeners.size(); i++) attachControllerStateChangedListener(listeners.get(i)); }
SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); controllerFactory = new MomentumBasedControllerFactory(contactableBodiesFactory, feetForceSensorNames, feetContactSensorNames, wristForceSensorNames, walkingControllerParameters, armControllerParameters, capturePointPlannerParameters, HighLevelState.WALKING); controllerFactory.attachControllerFailureListeners(controllerFailureListeners); controllerFactory.setICPOptimizationControllerParameters(icpOptimizationParameters); if (setupControllerNetworkSubscriber) controllerFactory.createControllerNetworkSubscriber(new PeriodicNonRealtimeThreadScheduler("CapturabilityBasedStatusProducer"), controllerPacketCommunicator); controllerFactory.addHighLevelBehaviorFactory(highLevelBehaviorFactories.get(i)); controllerFactory.setFallbackControllerForFailure(null); controllerFactory.createQueuedControllerCommandGenerator(controllerCommands); controllerFactory.createComponentBasedFootstepDataMessageGenerator(useHeadingAndVelocityScript, scsInitialSetup.getHeightMap()); else if (addFootstepMessageGenerator) controllerFactory.createComponentBasedFootstepDataMessageGenerator(useHeadingAndVelocityScript);
double totalRobotWeight = totalMass * gravityZ; SideDependentList<FootSwitchInterface> footSwitches = createFootSwitches(feet, forceSensorDataHolder, contactSensorHolder, totalRobotWeight, yoGraphicsListRegistry, registry); SideDependentList<ForceSensorDataReadOnly> wristForceSensors = createWristForceSensors(forceSensorDataHolder); 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(); highLevelHumanoidControllerManager.setListenToHighLevelStatePackets(isListeningToHighLevelStatePackets); createRegisteredControllers();
SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); MomentumBasedControllerFactory controllerFactory = new MomentumBasedControllerFactory(contactableBodiesFactory, feetForceSensorNames, feetContactSensorNames, wristForceSensorNames, walkingControllerParameters, armControllerParameters, capturePointPlannerParameters, HighLevelState.WALKING); controllerFactory.setICPOptimizationControllerParameters(icpOptimizationParameters); controllerFactory.setHeadingAndVelocityEvaluationScriptParameters(walkingScriptParameters); controllerFactory.createComponentBasedFootstepDataMessageGenerator(useVelocityAndHeadingScript, heightMapForFootstepZ);
SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); MomentumBasedControllerFactory controllerFactory = new MomentumBasedControllerFactory(contactableBodiesFactory, feetForceSensorNames, feetContactSensorNames, wristForceSensorNames, walkingControllerParameters, armControllerParameters, capturePointPlannerParameters, HighLevelState.WALKING); controllerFactory.setICPOptimizationControllerParameters(icpOptimizationParameters); controllerFactory.createComponentBasedFootstepDataMessageGenerator(useVelocityAndHeadingScript, heightMapForFootstepZ);
SideDependentList<String> feetContactSensorNames = sensorInformation.getFeetContactSensorNames(); SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); MomentumBasedControllerFactory momentumBasedControllerFactory = new MomentumBasedControllerFactory(contactableBodiesFactory, footSensorNames, feetContactSensorNames, wristForceSensorNames, walkingControllerParameters, armControllerParameters, capturePointPlannerParameters, HighLevelState.DO_NOTHING_BEHAVIOR); DiagnosticsWhenHangingControllerFactory diagnosticsWhenHangingControllerFactory = new DiagnosticsWhenHangingControllerFactory(humanoidJointPoseList, useArms, robotIsHanging, null); diagnosticsWhenHangingControllerFactory.setTransitionRequested(true); momentumBasedControllerFactory.addHighLevelBehaviorFactory(diagnosticsWhenHangingControllerFactory); momentumBasedControllerFactory.setICPOptimizationControllerParameters(icpOptimizationParameters);
private MomentumBasedControllerFactory createDRCControllerFactory(DRCRobotModel robotModel, PacketCommunicator packetCommunicator) { ContactableBodiesFactory contactableBodiesFactory = robotModel.getContactPointParameters().getContactableBodiesFactory(); ArmControllerParameters armControllerParameters = robotModel.getArmControllerParameters(); WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters(); final HighLevelState initialBehavior; CapturePointPlannerParameters capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters(); ICPOptimizationParameters icpOptimizationParameters = robotModel.getICPOptimizationParameters(); initialBehavior = HighLevelState.WALKING; // HERE!! SideDependentList<String> feetContactSensorNames = sensorInformation.getFeetContactSensorNames(); SideDependentList<String> feetForceSensorNames = sensorInformation.getFeetForceSensorNames(); SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); MomentumBasedControllerFactory controllerFactory = new MomentumBasedControllerFactory(contactableBodiesFactory, feetForceSensorNames, feetContactSensorNames, wristForceSensorNames, walkingControllerParameters, armControllerParameters, capturePointPlannerParameters, initialBehavior); controllerFactory.createControllerNetworkSubscriber(new PeriodicNonRealtimeThreadScheduler("CapturabilityBasedStatusProducer"), packetCommunicator); controllerFactory.setICPOptimizationControllerParameters(icpOptimizationParameters); // controllerFactory.addHighLevelBehaviorFactory(new JointPositionControllerFactory(true)); if (!USE_GUI) controllerFactory.createComponentBasedFootstepDataMessageGenerator(true, null); return controllerFactory; } }
SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); controllerFactory = new MomentumBasedControllerFactory(contactableBodiesFactory, feetForceSensorNames, feetContactSensorNames, wristForceSensorNames, walkingControllerParameters, armControllerParameters, capturePointPlannerParameters, HighLevelState.WALKING); controllerFactory.attachControllerFailureListeners(controllerFailureListeners); controllerFactory.setICPOptimizationControllerParameters(icpOptimizationParameters); if (setupControllerNetworkSubscriber) controllerFactory.createControllerNetworkSubscriber(new PeriodicNonRealtimeThreadScheduler("CapturabilityBasedStatusProducer"), controllerPacketCommunicator); controllerFactory.addHighLevelBehaviorFactory(highLevelBehaviorFactories.get(i)); controllerFactory.setFallbackControllerForFailure(null); controllerFactory.createQueuedControllerCommandGenerator(controllerCommands); controllerFactory.setHeadingAndVelocityEvaluationScriptParameters(walkingScriptParameters); controllerFactory.createComponentBasedFootstepDataMessageGenerator(useHeadingAndVelocityScript, scsInitialSetup.getHeightMap()); else if (addFootstepMessageGenerator) controllerFactory.createComponentBasedFootstepDataMessageGenerator(useHeadingAndVelocityScript);
SideDependentList<String> feetContactSensorNames = sensorInformation.getFeetContactSensorNames(); SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); MomentumBasedControllerFactory momentumBasedControllerFactory = new MomentumBasedControllerFactory(contactableBodiesFactory, footSensorNames, feetContactSensorNames, wristForceSensorNames, walkingControllerParameters, armControllerParameters, capturePointPlannerParameters, HighLevelState.DO_NOTHING_BEHAVIOR); DiagnosticsWhenHangingControllerFactory diagnosticsWhenHangingControllerFactory = new DiagnosticsWhenHangingControllerFactory(humanoidJointPoseList, useArms, robotIsHanging, null); diagnosticsWhenHangingControllerFactory.setTransitionRequested(true); momentumBasedControllerFactory.addHighLevelBehaviorFactory(diagnosticsWhenHangingControllerFactory); momentumBasedControllerFactory.setICPOptimizationControllerParameters(icpOptimizationParameters);
public void attachControllerFailureListener(ControllerFailureListener listener) { if (controllerFactory == null) this.controllerFailureListeners.add(listener); else controllerFactory.attachControllerFailureListener(listener); }
public void createComponentBasedFootstepDataMessageGenerator(boolean useHeadingAndVelocityScript) { createComponentBasedFootstepDataMessageGenerator(useHeadingAndVelocityScript, null); }
private void setupSimulationOutputWriter() { simulationOutputWriter = new DRCSimulationOutputWriter(humanoidFloatingRootJointRobot); if (doSmoothJointTorquesAtControllerStateChanges.get()) { DRCOutputWriterWithStateChangeSmoother drcOutputWriterWithStateChangeSmoother = new DRCOutputWriterWithStateChangeSmoother(simulationOutputWriter); momentumBasedControllerFactory.get() .attachControllerStateChangedListener(drcOutputWriterWithStateChangeSmoother.createControllerStateChangedListener()); simulationOutputWriter = drcOutputWriterWithStateChangeSmoother; } if (doSlowIntegrationForTorqueOffset.get()) { DRCOutputWriterWithTorqueOffsets outputWriterWithTorqueOffsets = new DRCOutputWriterWithTorqueOffsets(simulationOutputWriter, robotModel.get().getControllerDT()); simulationOutputWriter = outputWriterWithTorqueOffsets; } }
public void attachControllerFailureListeners(List<ControllerFailureListener> listeners) { for (int i = 0; i < listeners.size(); i++) attachControllerFailureListener(listeners.get(i)); }
public void attachControllerFailureListener(ControllerFailureListener listener) { drcSimulation.getControllerFactory().attachControllerFailureListener(listener); }
public void attachControllerFailureListener(ControllerFailureListener listener) { avatarSimulation.getMomentumBasedControllerFactory().attachControllerFailureListener(listener); }