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 DRCSimulationStarter(DRCRobotModel robotModel, CommonAvatarEnvironmentInterface environment) { this.robotModel = robotModel; this.environment = environment; this.guiInitialSetup = new DRCGuiInitialSetup(false, false); this.robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0); this.createSCSSimulatedSensors = true; scsInitialSetup = new DRCSCSInitialSetup(environment, robotModel.getSimulateDT()); scsInitialSetup.setInitializeEstimatorToActual(false); scsInitialSetup.setTimePerRecordTick(robotModel.getControllerDT()); scsInitialSetup.setRunMultiThreaded(true); this.walkingControllerParameters = robotModel.getWalkingControllerParameters(); this.armControllerParameters = robotModel.getArmControllerParameters(); this.capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters(); this.icpOptimizationParameters = robotModel.getICPOptimizationParameters(); this.contactPointParameters = robotModel.getContactPointParameters(); }
int estimatorTicksPerSimulationTick = (int) Math.round(robotModel.getEstimatorDT() / robotModel.getEstimatorDT()); int controllerTicksPerSimulationTick = (int) Math.round(robotModel.getControllerDT() / robotModel.getEstimatorDT()); robotModel.getEstimatorDT()); DRCEstimatorThread estimatorThread = new DRCEstimatorThread(robotModel.getSimpleRobotName(), robotModel.getSensorInformation(), robotModel.getContactPointParameters(), robotModel, robotModel.getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, realtimeRos2Node, null, null, robotVisualizer, gravity);
public StepprSensorReaderFactory(DRCRobotModel robotModel) { sensorInformation = robotModel.getSensorInformation(); stateEstimatorParameters = robotModel.getStateEstimatorParameters(); }
public static void main(String[] args) throws IOException { DRCRobotModel robotModel = new ValkyrieRobotModel(RobotTarget.SCS, false); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); FullHumanoidRobotModel fullRobotModelForSlider = robotModel.createFullRobotModel(); new PosePlaybackSCSBridge(sdfRobot, fullRobotModel, fullRobotModelForSlider, robotModel.getControllerDT()); } }
private void setupHeightQuadTreeToolboxModule(DRCRobotModel robotModel, DRCNetworkModuleParameters params) throws IOException { if (params.isHeightQuadTreeToolboxEnabled()) new HeightQuadTreeToolboxModule(robotModel.getSimpleRobotName(), robotModel.createFullRobotModel(), robotModel.getLogModelProvider()); }
private DRCSimulationStarter(DRCRobotModel robotModel, CommonAvatarEnvironmentInterface environment, GroundProfile3D groundProfile3D) { this.robotModel = robotModel; this.environment = environment; this.guiInitialSetup = new DRCGuiInitialSetup(false, false); this.robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0); this.createSCSSimulatedSensors = true; this.scsInitialSetup = new DRCSCSInitialSetup(groundProfile3D, robotModel.getSimulateDT()); this.scsInitialSetup.setDrawGroundProfile(environment == null); this.scsInitialSetup.setInitializeEstimatorToActual(false); this.scsInitialSetup.setTimePerRecordTick(robotModel.getControllerDT()); this.scsInitialSetup.setRunMultiThreaded(true); this.highLevelControllerParameters = robotModel.getHighLevelControllerParameters(); this.walkingControllerParameters = robotModel.getWalkingControllerParameters(); this.capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters(); this.contactPointParameters = robotModel.getContactPointParameters(); }
private void setupMultiThreadedRobotController() { if (scsInitialSetup.get().getRunMultiThreaded()) { threadedRobotController = new MultiThreadedRobotController("DRCSimulation", humanoidFloatingRootJointRobot, simulationConstructionSet); } else { PrintTools.warn(this, "Running simulation in single threaded mode", true); threadedRobotController = new SingleThreadedRobotController("DRCSimulation", humanoidFloatingRootJointRobot, simulationConstructionSet); } int estimatorTicksPerSimulationTick = (int) Math.round(robotModel.get().getEstimatorDT() / robotModel.get().getSimulateDT()); int controllerTicksPerSimulationTick = (int) Math.round(robotModel.get().getControllerDT() / robotModel.get().getSimulateDT()); int slowPublisherTicksPerSimulationTick = (int) Math.round(10 * robotModel.get().getEstimatorDT() / robotModel.get().getSimulateDT()); threadedRobotController.addController(stateEstimationThread, estimatorTicksPerSimulationTick, false); threadedRobotController.addController(controllerThread, controllerTicksPerSimulationTick, true); MultiThreadedRobotControlElement simulatedHandController = robotModel.get().createSimulatedHandController(humanoidFloatingRootJointRobot, threadDataSynchronizer, realtimeRos2Node.get(), closeableAndDisposableRegistry); if (simulatedHandController != null) { threadedRobotController.addController(simulatedHandController, controllerTicksPerSimulationTick, false); } }
private FullHumanoidRobotModel createFullRobotModelAtInitialConfiguration() { DRCRobotModel robotModel = getRobotModel(); FullHumanoidRobotModel initialFullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = robotModel.createHumanoidFloatingRootJointRobot(false); robotModel.getDefaultRobotInitialSetup(0.0, 0.0).initializeRobot(robot, robotModel.getJointMap()); DRCPerfectSensorReaderFactory drcPerfectSensorReaderFactory = new DRCPerfectSensorReaderFactory(robot, null, 0); drcPerfectSensorReaderFactory.build(initialFullRobotModel.getRootJoint(), null, null, null, null, null, null); drcPerfectSensorReaderFactory.getSensorReader().read(); return initialFullRobotModel; }
public DRCFlatGroundWalkingTrack(DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup, DRCGuiInitialSetup guiInitialSetup, DRCSCSInitialSetup scsInitialSetup, boolean useVelocityAndHeadingScript, boolean cheatWithGroundHeightAtForFootstep, DRCRobotModel model, WalkingProvider walkingProvider, HeadingAndVelocityEvaluationScriptParameters walkingScriptParameters) ArmControllerParameters armControllerParameters = model.getArmControllerParameters(); int recordFrequency = (int) Math.round(model.getControllerDT() / dt); if (recordFrequency < 1) recordFrequency = 1; scsInitialSetup.setRecordFrequency(recordFrequency); WalkingControllerParameters walkingControllerParameters = model.getWalkingControllerParameters(); RobotContactPointParameters contactPointParameters = model.getContactPointParameters(); CapturePointPlannerParameters capturePointPlannerParameters = model.getCapturePointPlannerParameters(); ICPOptimizationParameters icpOptimizationParameters = model.getICPOptimizationParameters(); ContactableBodiesFactory contactableBodiesFactory = contactPointParameters.getContactableBodiesFactory(); DRCRobotSensorInformation sensorInformation = model.getSensorInformation(); SideDependentList<String> feetForceSensorNames = sensorInformation.getFeetForceSensorNames(); SideDependentList<String> feetContactSensorNames = sensorInformation.getFeetContactSensorNames();
int recordFrequency = (int) Math.round(model.getControllerDT() / dt); if (recordFrequency < 1) recordFrequency = 1; scsInitialSetup.setRecordFrequency(recordFrequency); HighLevelControllerParameters highLevelControllerParameters = model.getHighLevelControllerParameters(); WalkingControllerParameters walkingControllerParameters = model.getWalkingControllerParameters(); ICPWithTimeFreezingPlannerParameters capturePointPlannerParameters = model.getCapturePointPlannerParameters(); DRCRobotSensorInformation sensorInformation = model.getSensorInformation(); SideDependentList<String> feetForceSensorNames = sensorInformation.getFeetForceSensorNames(); SideDependentList<String> feetContactSensorNames = sensorInformation.getFeetContactSensorNames(); SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames(); RobotContactPointParameters<RobotSide> contactPointParameters = model.getContactPointParameters(); ArrayList<String> additionalContactRigidBodyNames = contactPointParameters.getAdditionalContactRigidBodyNames(); ArrayList<String> additionalContactNames = contactPointParameters.getAdditionalContactNames();
private HighLevelHumanoidControllerFactory createHighLevelHumanoidControllerFactory(DRCRobotModel robotModel, PacketCommunicator packetCommunicator) RobotContactPointParameters<RobotSide> contactPointParameters = robotModel.getContactPointParameters(); ArrayList<String> additionalContactRigidBodyNames = contactPointParameters.getAdditionalContactRigidBodyNames(); ArrayList<String> additionalContactNames = contactPointParameters.getAdditionalContactNames(); additionalContactTransforms.get(i)); WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters(); ICPWithTimeFreezingPlannerParameters capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters(); HighLevelControllerParameters highLevelControllerParameters = robotModel.getHighLevelControllerParameters(); DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation(); SideDependentList<String> feetForceSensorNames = sensorInformation.getFeetForceSensorNames(); SideDependentList<String> feetContactSensorNames = sensorInformation.getFeetContactSensorNames(); capturePointPlannerParameters); controllerFactory.createControllerNetworkSubscriber(robotModel.getSimpleRobotName(), realtimeRos2Node);
DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(environment, model.getSimulateDT()); scsInitialSetup.setRunMultiThreaded(false); robotInitialSetup.setInitialGroundHeight(0.0); RobotContactPointParameters<RobotSide> contactPointParameters = model.getContactPointParameters(); ArrayList<String> additionalContactRigidBodyNames = contactPointParameters.getAdditionalContactRigidBodyNames(); ArrayList<String> additionalContactNames = contactPointParameters.getAdditionalContactNames(); additionalContactTransforms.get(i)); DRCRobotSensorInformation sensorInformation = model.getSensorInformation(); SideDependentList<String> footSensorNames = sensorInformation.getFeetForceSensorNames(); HighLevelControllerParameters highLevelControllerParameters = model.getHighLevelControllerParameters(); WalkingControllerParameters walkingControllerParameters = model.getWalkingControllerParameters(); ICPWithTimeFreezingPlannerParameters capturePointPlannerParameters = model.getCapturePointPlannerParameters(); SideDependentList<String> feetContactSensorNames = sensorInformation.getFeetContactSensorNames(); SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames();
DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(environment, model.getSimulateDT()); scsInitialSetup.setRunMultiThreaded(false); robotInitialSetup.setInitialGroundHeight(0.0); ContactableBodiesFactory contactableBodiesFactory = model.getContactPointParameters().getContactableBodiesFactory(); DRCRobotSensorInformation sensorInformation = model.getSensorInformation(); SideDependentList<String> footSensorNames = sensorInformation.getFeetForceSensorNames(); WalkingControllerParameters walkingControllerParameters = model.getWalkingControllerParameters(); ArmControllerParameters armControllerParameters = model.getArmControllerParameters(); CapturePointPlannerParameters capturePointPlannerParameters = model.getCapturePointPlannerParameters(); ICPOptimizationParameters icpOptimizationParameters = model.getICPOptimizationParameters(); SideDependentList<String> feetContactSensorNames = sensorInformation.getFeetContactSensorNames(); SideDependentList<String> wristForceSensorNames = sensorInformation.getWristForceSensorNames();
public FootstepPlanningToolboxModule(DRCRobotModel drcRobotModel, LogModelProvider modelProvider, boolean startYoVariableServer, DomainFactory.PubSubImplementation pubSubImplementation) throws IOException { super(drcRobotModel.getSimpleRobotName(), drcRobotModel.createFullRobotModel(), modelProvider, startYoVariableServer, pubSubImplementation); setTimeWithoutInputsBeforeGoingToSleep(Double.POSITIVE_INFINITY); footstepPlanningToolboxController = new FootstepPlanningToolboxController(drcRobotModel.getContactPointParameters(), drcRobotModel.getFootstepPlannerParameters(), drcRobotModel.getVisibilityGraphsParameters(), statusOutputManager, registry, yoGraphicsListRegistry, Conversions.millisecondsToSeconds(DEFAULT_UPDATE_PERIOD_MILLISECONDS)); footstepPlanningToolboxController.setTextToSpeechPublisher(textToSpeechPublisher); startYoVariableServer(); }
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; ContactableBodiesFactory contactableBodiesFactory = robotModel.getContactPointParameters().getContactableBodiesFactory(); CommonHumanoidReferenceFrames referenceFrames = new HumanoidReferenceFrames(estimatorFullRobotModel); SideDependentList<? extends ContactablePlaneBody> bipedFeet = contactableBodiesFactory.createFootContactableBodies(estimatorFullRobotModel, referenceFrames);
private void setupTrack(boolean runMultiThreaded, DRCRobotModel robotModel) { DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false, simulationTestingParameters); GroundProfile3D groundProfile = new FlatGroundProfile(); DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT()); scsInitialSetup.setInitializeEstimatorToActual(true); scsInitialSetup.setDrawGroundProfile(true); scsInitialSetup.setRunMultiThreaded(runMultiThreaded); DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0); drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, true, false, robotModel); }
public DRCInverseDynamicsCalculatorTestHelper createInverseDynamicsCalculatorTestHelper(boolean visualize, double gravityZ) { DRCRobotModel drcRobotModel = getRobotModel(); FullHumanoidRobotModel fullRobotModel = drcRobotModel.createFullRobotModel(); boolean createCollisionMeshes = false; boolean enableJointDamping = false; HumanoidFloatingRootJointRobot robot = drcRobotModel.createHumanoidFloatingRootJointRobot(createCollisionMeshes, enableJointDamping); robot.setGravity(gravityZ); return new DRCInverseDynamicsCalculatorTestHelper(fullRobotModel, robot, visualize, gravityZ); } }
private HighLevelHumanoidControllerFactory createDRCControllerFactory(DRCRobotModel robotModel, PacketCommunicator packetCommunicator, DRCRobotSensorInformation sensorInformation) ContactableBodiesFactory contactableBodiesFactory = robotModel.getContactPointParameters().getContactableBodiesFactory(); HighLevelControllerParameters highLevelControllerParameters = robotModel.getHighLevelControllerParameters(); WalkingControllerParameters walkingControllerParamaters = robotModel.getWalkingControllerParameters(); ICPWithTimeFreezingPlannerParameters capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters();
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()); }