public StepprSensorReaderFactory(DRCRobotModel robotModel) { sensorInformation = robotModel.getSensorInformation(); stateEstimatorParameters = robotModel.getStateEstimatorParameters(); }
public WandererSensorReaderFactory(DRCRobotModel robotModel, CostOfTransportCalculator costOfTransportCalculator) { sensorInformation = robotModel.getSensorInformation(); stateEstimatorParameters = robotModel.getStateEstimatorParameters(); this.costOfTransportCalculator = costOfTransportCalculator; }
private void setupSensorReaderFactory() { if (scsInitialSetup.get().usePerfectSensors()) { sensorReaderFactory = new DRCPerfectSensorReaderFactory(humanoidFloatingRootJointRobot, threadDataSynchronizer.getEstimatorForceSensorDataHolder(), robotModel.get().getStateEstimatorParameters().getEstimatorDT()); } else { sensorReaderFactory = new SimulatedSensorHolderAndReaderFromRobotFactory(humanoidFloatingRootJointRobot, robotModel.get().getStateEstimatorParameters()); } }
private void setupSensorReaderFactory() { if (scsInitialSetup.get().usePerfectSensors()) { sensorReaderFactory = new DRCPerfectSensorReaderFactory(humanoidFloatingRootJointRobot, threadDataSynchronizer.getEstimatorForceSensorDataHolder(), robotModel.get().getStateEstimatorParameters().getEstimatorDT()); } else { sensorReaderFactory = new SimulatedSensorHolderAndReaderFromRobotFactory(humanoidFloatingRootJointRobot, robotModel.get().getStateEstimatorParameters()); } }
private DRCKinematicsBasedStateEstimator createStateEstimator(FullInverseDynamicsStructure inverseDynamicsStructure, DRCRobotModel robotModel, final LogDataProcessorHelper logDataProcessorHelper, SensorOutputMapReadOnly postProcessedSensors) StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters(); String[] imuSensorsToUseInStateEstimator = robotModel.getSensorInformation().getIMUSensorsToUseInStateEstimator(); double gravitationalAcceleration = -9.80;
robotModel.getContactPointParameters(), robotModel, robotModel.getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, realtimeRos2Node, null, null, robotVisualizer, gravity);
RobotContactPointParameters<RobotSide> contactPointParameters = robotModel.getContactPointParameters(); DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation(); StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters();
private void setupStateEstimationThread() { stateEstimationThread = new DRCEstimatorThread(robotModel.get().getSensorInformation(), robotModel.get().getContactPointParameters(), robotModel.get().getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, new PeriodicNonRealtimeThreadScheduler("DRCSimGazeboYoVariableServer"), humanoidGlobalDataProducer.get(), yoVariableServer, gravity.get()); if (humanoidGlobalDataProducer.get() != null) { PelvisPoseCorrectionCommunicatorInterface pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(humanoidGlobalDataProducer.get()); humanoidGlobalDataProducer.get().attachListener(StampedPosePacket.class, pelvisPoseCorrectionCommunicator); stateEstimationThread.setExternalPelvisCorrectorSubscriber(pelvisPoseCorrectionCommunicator); } else { stateEstimationThread.setExternalPelvisCorrectorSubscriber(null); } }
public AutomatedDiagnosticConfiguration createDiagnosticController(boolean startWithRobotAlive) { simulatedRobot = robotModel.createHumanoidFloatingRootJointRobot(false); DiagnosticLoggerConfiguration.setupLogging(simulatedRobot.getYoTime(), getClass(), robotModel.getSimpleRobotName()); if (robotInitialSetup == null) robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); robotInitialSetup.initializeRobot(simulatedRobot, robotModel.getJointMap()); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters(); DoubleYoVariable yoTime = simulatedRobot.getYoTime(); double dt = robotModel.getEstimatorDT(); StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters(); if (diagnosticParameters == null) diagnosticParameters = new DiagnosticParameters(DiagnosticEnvironment.RUNTIME_CONTROLLER, false); DiagnosticSensorProcessingConfiguration sensorProcessingConfiguration = new DiagnosticSensorProcessingConfiguration(diagnosticParameters, stateEstimatorParameters); SensorOutputMapReadOnly sensorOutputMap = createStateEstimator(fullRobotModel, stateEstimatorParameters, sensorProcessingConfiguration); DiagnosticControllerToolbox diagnosticControllerToolbox = new DiagnosticControllerToolbox(fullRobotModel, sensorOutputMap, diagnosticParameters, walkingControllerParameters, yoTime, dt, sensorProcessingConfiguration, simulationRegistry); automatedDiagnosticAnalysisController = new AutomatedDiagnosticAnalysisController(diagnosticControllerToolbox, gainStream, setpointStream, simulationRegistry); automatedDiagnosticAnalysisController.setRobotIsAlive(startWithRobotAlive); automatedDiagnosticConfiguration = new AutomatedDiagnosticConfiguration(diagnosticControllerToolbox, automatedDiagnosticAnalysisController); outputWriter = new DRCSimulationOutputWriter(simulatedRobot); outputWriter.setFullRobotModel(fullRobotModel, null); int simulationTicksPerControlTick = (int) (robotModel.getEstimatorDT() / robotModel.getSimulateDT()); simulatedRobot.setController(this, simulationTicksPerControlTick); return automatedDiagnosticConfiguration; }
public AutomatedDiagnosticConfiguration createDiagnosticController(boolean startWithRobotAlive) { simulatedRobot = robotModel.createHumanoidFloatingRootJointRobot(false); DiagnosticLoggerConfiguration.setupLogging(simulatedRobot.getYoTime(), getClass(), robotModel.getSimpleRobotName()); if (robotInitialSetup == null) robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); robotInitialSetup.initializeRobot(simulatedRobot, robotModel.getJointMap()); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters(); YoDouble yoTime = simulatedRobot.getYoTime(); double dt = robotModel.getEstimatorDT(); StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters(); if (diagnosticParameters == null) diagnosticParameters = new DiagnosticParameters(DiagnosticEnvironment.RUNTIME_CONTROLLER, false); JointDesiredOutputList lowLevelOutput = new JointDesiredOutputList(fullRobotModel.getOneDoFJoints()); DiagnosticSensorProcessingConfiguration sensorProcessingConfiguration = new DiagnosticSensorProcessingConfiguration(diagnosticParameters, stateEstimatorParameters, lowLevelOutput); SensorOutputMapReadOnly sensorOutputMap = createStateEstimator(fullRobotModel, stateEstimatorParameters, sensorProcessingConfiguration); DiagnosticControllerToolbox diagnosticControllerToolbox = new DiagnosticControllerToolbox(fullRobotModel, lowLevelOutput, sensorOutputMap, diagnosticParameters, walkingControllerParameters, yoTime, dt, sensorProcessingConfiguration, simulationRegistry); automatedDiagnosticAnalysisController = new AutomatedDiagnosticAnalysisController(diagnosticControllerToolbox, gainStream, setpointStream, simulationRegistry); automatedDiagnosticAnalysisController.setRobotIsAlive(startWithRobotAlive); automatedDiagnosticConfiguration = new AutomatedDiagnosticConfiguration(diagnosticControllerToolbox, automatedDiagnosticAnalysisController); lowLevelOutputWriter = new SimulatedLowLevelOutputWriter(simulatedRobot, false); lowLevelOutputWriter.setJointDesiredOutputList(lowLevelOutput); int simulationTicksPerControlTick = (int) (robotModel.getEstimatorDT() / robotModel.getSimulateDT()); simulatedRobot.setController(this, simulationTicksPerControlTick); return automatedDiagnosticConfiguration; }
private void setupStateEstimationThread() { String robotName = robotModel.get().getSimpleRobotName(); MessageTopicNameGenerator publisherTopicNameGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); MessageTopicNameGenerator subscriberTopicNameGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName); PelvisPoseCorrectionCommunicatorInterface pelvisPoseCorrectionCommunicator; if (externalPelvisCorrectorSubscriber.hasValue()) { pelvisPoseCorrectionCommunicator = externalPelvisCorrectorSubscriber.get(); } else { pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(realtimeRos2Node.get(), publisherTopicNameGenerator); ROS2Tools.createCallbackSubscription(realtimeRos2Node.get(), StampedPosePacket.class, subscriberTopicNameGenerator, s -> pelvisPoseCorrectionCommunicator.receivedPacket(s.takeNextData())); } stateEstimationThread = new DRCEstimatorThread(robotName, robotModel.get().getSensorInformation(), robotModel.get().getContactPointParameters(), robotModel.get(), robotModel.get().getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, realtimeRos2Node.get(), pelvisPoseCorrectionCommunicator, simulationOutputWriter, yoVariableServer, gravity.get()); }