public ZeroPoseMockRobotConfigurationDataPublisherModule(final DRCRobotModel robotModel) { fullRobotModel = robotModel.createFullRobotModel(); forceSensorDefinitions = fullRobotModel.getForceSensorDefinitions(); publisher = ROS2Tools.createPublisher(ros2Node, RobotConfigurationData.class, ControllerAPIDefinition.getPublisherTopicNameGenerator(robotModel.getSimpleRobotName())); Thread t = new Thread(this); t.start(); }
private void setupHeightQuadTreeToolboxModule(DRCRobotModel robotModel, DRCNetworkModuleParameters params) throws IOException { if (params.isHeightQuadTreeToolboxEnabled()) new HeightQuadTreeToolboxModule(robotModel.getSimpleRobotName(), robotModel.createFullRobotModel(), robotModel.getLogModelProvider()); }
private SideDependentList<WristForceSensorFilteredUpdatable> createWristForceSensorUpdateables() { SideDependentList<WristForceSensorFilteredUpdatable> ret = new SideDependentList<WristForceSensorFilteredUpdatable>(); for (RobotSide robotSide : RobotSide.values) { WristForceSensorFilteredUpdatable wristSensorUpdatable = new WristForceSensorFilteredUpdatable(drcRobotModel.getSimpleRobotName(), robotSide, fullRobotModel, drcRobotModel.getSensorInformation(), robotDataReceiver.getForceSensorDataHolder(), IHMCHumanoidBehaviorManager.BEHAVIOR_YO_VARIABLE_SERVER_DT, ros2Node, registry); ret.put(robotSide, wristSensorUpdatable); } return ret; }
public KinematicsToolboxModule(DRCRobotModel robotModel, boolean startYoVariableServer, PubSubImplementation pubSubImplementation) throws IOException { super(robotModel.getSimpleRobotName(), robotModel.createFullRobotModel(), robotModel.getLogModelProvider(), startYoVariableServer, DEFAULT_UPDATE_PERIOD_MILLISECONDS, pubSubImplementation); kinematicsToolBoxController = new HumanoidKinematicsToolboxController(commandInputManager, statusOutputManager, fullRobotModel, yoGraphicsListRegistry, registry); commandInputManager.registerConversionHelper(new KinematicsToolboxCommandConverter(fullRobotModel)); startYoVariableServer(); }
public KinematicsPlanningToolboxModule(DRCRobotModel robotModel, boolean startYoVariableServer, PubSubImplementation pubSubImplementation) throws IOException { super(robotModel.getSimpleRobotName(), robotModel.createFullRobotModel(), robotModel.getLogModelProvider(), startYoVariableServer, DEFAULT_UPDATE_PERIOD_MILLISECONDS, pubSubImplementation); kinematicsPlanningToolboxController = new KinematicsPlanningToolboxController(robotModel, fullRobotModel, commandInputManager, statusOutputManager, yoGraphicsListRegistry, registry); commandInputManager.registerConversionHelper(new KinematicsPlanningToolboxCommandConverter(fullRobotModel)); startYoVariableServer(); }
private void setupBehaviorModule(DRCRobotModel robotModel, DRCNetworkModuleParameters params) throws IOException { if (params.isBehaviorModuleEnabled()) { DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation(); LogModelProvider logModelProvider = robotModel.getLogModelProvider(); if (params.isAutomaticDiagnosticEnabled()) { IHMCHumanoidBehaviorManager.createBehaviorModuleForAutomaticDiagnostic(robotModel.getSimpleRobotName(), robotModel, robotModel, logModelProvider, params.isBehaviorVisualizerEnabled(), sensorInformation, params.getTimeToWaitBeforeStartingDiagnostics()); } else { new IHMCHumanoidBehaviorManager(robotModel.getSimpleRobotName(), robotModel, robotModel, logModelProvider, params.isBehaviorVisualizerEnabled(), sensorInformation); } String methodName = "setupBehaviorModule "; printModuleConnectedDebugStatement(PacketDestination.BEHAVIOR_MODULE, methodName); } }
@Override public void start(Stage primaryStage) throws Exception { DRCRobotModel drcRobotModel = new ValkyrieRobotModel(RobotTarget.REAL_ROBOT, false); messager = new SharedMemoryJavaFXMessager(FootstepPlannerMessagerAPI.API); messageConverter = RemoteUIMessageConverter.createConverter(messager, drcRobotModel.getSimpleRobotName(), DomainFactory.PubSubImplementation.FAST_RTPS); messager.startMessager(); ui = FootstepPlannerUI.createMessagerUI(primaryStage, messager, drcRobotModel.getFootstepPlannerParameters(), drcRobotModel.getVisibilityGraphsParameters()); ui.show(); }
public WholeBodyTrajectoryToolboxModule(DRCRobotModel drcRobotModel, boolean startYoVariableServer, PubSubImplementation pubSubImplementation) throws IOException { super(drcRobotModel.getSimpleRobotName(), drcRobotModel.createFullRobotModel(), drcRobotModel.getLogModelProvider(), startYoVariableServer, DEFAULT_UPDATE_PERIOD_MILLISECONDS, pubSubImplementation); setTimeWithoutInputsBeforeGoingToSleep(Double.POSITIVE_INFINITY); wholeBodyTrajectoryToolboxController = new WholeBodyTrajectoryToolboxController(drcRobotModel, fullRobotModel, commandInputManager, statusOutputManager, registry, yoGraphicsListRegistry, startYoVariableServer); controllerNetworkSubscriber.registerSubcriberWithMessageUnpacker(WholeBodyTrajectoryToolboxMessage.class, 10, MessageUnpackingTools.createWholeBodyTrajectoryToolboxMessageUnpacker()); commandInputManager.registerConversionHelper(new WholeBodyTrajectoryToolboxCommandConverter(fullRobotModel)); }
public DRCSimulationTestHelper(SimulationTestingParameters simulationTestParameters, DRCRobotModel robotModel, CommonAvatarEnvironmentInterface testEnvironment) { this.robotModel = robotModel; this.walkingControlParameters = robotModel.getWalkingControllerParameters(); this.simulationTestingParameters = simulationTestParameters; robotName = robotModel.getSimpleRobotName(); if (testEnvironment != null) this.testEnvironment = testEnvironment; simulationStarter = new DRCSimulationStarter(robotModel, this.testEnvironment); fullRobotModel = robotModel.createFullRobotModel(); HumanoidReferenceFrames referenceFrames = new HumanoidReferenceFrames(fullRobotModel); scriptedFootstepGenerator = new ScriptedFootstepGenerator(referenceFrames, fullRobotModel, walkingControlParameters); guiInitialSetup = new DRCGuiInitialSetup(false, false, simulationTestingParameters); networkProcessorParameters.enableNetworkProcessor(false); networkProcessorParameters.enableLocalControllerCommunicator(true); List<Class<? extends Command<?, ?>>> controllerSupportedCommands = ControllerAPIDefinition.getControllerSupportedCommands(); for (Class<? extends Command<?, ?>> command : controllerSupportedCommands) { Class<?> messageClass = ROS2Tools.newMessageInstance(command).getMessageClass(); IHMCROS2Publisher<?> defaultPublisher = createPublisherForController(messageClass); defaultControllerPublishers.put(messageClass, defaultPublisher); } defaultControllerPublishers.put(WholeBodyTrajectoryMessage.class, createPublisherForController(WholeBodyTrajectoryMessage.class)); defaultControllerPublishers.put(MessageCollection.class, createPublisherForController(MessageCollection.class)); defaultControllerPublishers.put(ValkyrieHandFingerTrajectoryMessage.class, createPublisherForController(ValkyrieHandFingerTrajectoryMessage.class)); }
robotModel.getEstimatorDT()); DRCEstimatorThread estimatorThread = new DRCEstimatorThread(robotModel.getSimpleRobotName(), robotModel.getSensorInformation(), robotModel.getContactPointParameters(), robotModel, robotModel.getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, realtimeRos2Node, null, null, robotVisualizer,
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 void setupControllerThread() { String robotName = robotModel.get().getSimpleRobotName(); controllerThread = new DRCControllerThread(robotName, robotModel.get(), robotModel.get().getSensorInformation(), highLevelHumanoidControllerFactory.get(), threadDataSynchronizer, simulationOutputProcessor, realtimeRos2Node.get(), yoVariableServer, gravity.get(), robotModel.get().getEstimatorDT()); }
@Before public void setUp() { registry = new YoVariableRegistry(getClass().getSimpleName()); this.yoTime = new YoDouble("yoTime", registry); this.ros2Node = ROS2Tools.createRos2Node(PubSubImplementation.INTRAPROCESS, "ihmc_humanoid_behavior_dispatcher_test"); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.createSimulation(getSimpleRobotName()); MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(getClass().getSimpleName() + " before test."); robot = drcSimulationTestHelper.getRobot(); fullRobotModel = getRobotModel().createFullRobotModel(); walkingControllerParameters = getRobotModel().getWalkingControllerParameters(); yoGraphicsListRegistry = new YoGraphicsListRegistry(); behaviorDispatcher = setupBehaviorDispatcher(getRobotModel().getSimpleRobotName(), fullRobotModel, ros2Node, yoGraphicsListRegistry, registry); CapturePointUpdatable capturePointUpdatable = createCapturePointUpdateable(drcSimulationTestHelper, registry, yoGraphicsListRegistry); behaviorDispatcher.addUpdatable(capturePointUpdatable); DRCRobotSensorInformation sensorInfo = getRobotModel().getSensorInformation(); for (RobotSide robotSide : RobotSide.values) { WristForceSensorFilteredUpdatable wristSensorUpdatable = new WristForceSensorFilteredUpdatable(drcSimulationTestHelper.getRobotName(), robotSide, fullRobotModel, sensorInfo, robotDataReceiver.getForceSensorDataHolder(), IHMCHumanoidBehaviorManager.BEHAVIOR_YO_VARIABLE_SERVER_DT, drcSimulationTestHelper.getRos2Node(), registry); behaviorDispatcher.addUpdatable(wristSensorUpdatable); } referenceFrames = robotDataReceiver.getReferenceFrames(); }
capturePointPlannerParameters); controllerFactory.createControllerNetworkSubscriber(robotModel.getSimpleRobotName(), realtimeRos2Node);
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; }
WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters(); Ros2Node ros2Node = drcBehaviorTestHelper.getRos2Node(); CollaborativeBehavior collaborativeBehavior = new CollaborativeBehavior(robotModel.getSimpleRobotName(), ros2Node, referenceFrames, fullRobotModel, robotSensorInfo, walkingControllerParameters, null); scs.addYoVariableRegistry(collaborativeBehavior.getYoVariableRegistry());
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()); }
public void runFlatGroundWalking() throws SimulationExceededMaximumTimeException, ControllerFailureException { DRCRobotModel robotModel = getRobotModel(); boolean doPelvisWarmup = doPelvisWarmup(); BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); FlatGroundEnvironment flatGround = new FlatGroundEnvironment(); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.setTestEnvironment(flatGround); drcSimulationTestHelper.setAddFootstepMessageGenerator(true); drcSimulationTestHelper.setUseHeadingAndVelocityScript(useVelocityAndHeadingScript); drcSimulationTestHelper.setCheatWithGroundHeightAtFootstep(cheatWithGroundHeightAtForFootstep); drcSimulationTestHelper.setWalkingScriptParameters(getWalkingScriptParameters()); drcSimulationTestHelper.createSimulation(robotModel.getSimpleRobotName() + "FlatGroundWalking"); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); setupCameraForUnitTest(scs); simulateAndAssertGoodWalking(drcSimulationTestHelper, doPelvisWarmup); if (simulationTestingParameters.getCheckNothingChangedInSimulation()) drcSimulationTestHelper.checkNothingChanged(); if (CHECK_ICP_CONTINUITY) verifyDesiredICPIsContinous(scs); createVideo(scs); BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows()); }
RobotMotionStatusHolder robotMotionStatusFromController = new RobotMotionStatusHolder(); DRCRobotSensorInformation sensorInformation = drcRobotModel.getSensorInformation(); MessageTopicNameGenerator publisherTopicNameGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(drcRobotModel.getSimpleRobotName()); final DRCPoseCommunicator poseCommunicator = new DRCPoseCommunicator(humanoidFullRobotModel, jointConfigurationGatherer, auxiliaryRobotDataProvider, publisherTopicNameGenerator, realtimeRos2Node, sensorOutputMapReadOnly,