- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {ScheduledThreadPoolExecutor s =
new ScheduledThreadPoolExecutor(corePoolSize)
- Smart code suggestions by Tabnine
}
public WandererSensorReaderFactory(DRCRobotModel robotModel, CostOfTransportCalculator costOfTransportCalculator) { sensorInformation = robotModel.getSensorInformation(); stateEstimatorParameters = robotModel.getStateEstimatorParameters(); this.costOfTransportCalculator = costOfTransportCalculator; }
public StepprSensorReaderFactory(DRCRobotModel robotModel) { sensorInformation = robotModel.getSensorInformation(); stateEstimatorParameters = robotModel.getStateEstimatorParameters(); }
protected void setupSubscribers(ConnectedNode connectedNode) { DRCRobotSensorInformation sensorInforamtion = robotModel.getSensorInformation(); DRCRobotLidarParameters lidarParameters = sensorInforamtion.getLidarParameters(0); if (lidarParameters != null && lidarParameters.getLidarSpindleJointTopic() != null) { jointSubscriber = connectedNode.newSubscriber(lidarParameters.getLidarSpindleJointTopic(), sensor_msgs.JointState._TYPE); } }
protected void setupSubscribers(ConnectedNode connectedNode) { DRCRobotSensorInformation sensorInforamtion = robotModel.getSensorInformation(); DRCRobotLidarParameters lidarParameters = sensorInforamtion.getLidarParameters(0); if (lidarParameters != null && lidarParameters.getLidarSpindleJointTopic() != null) { jointSubscriber = connectedNode.newSubscriber(lidarParameters.getLidarSpindleJointTopic(), sensor_msgs.JointState._TYPE); } }
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; }
private void setupControllerThread() { controllerThread = new DRCControllerThread(robotModel.get(), robotModel.get().getSensorInformation(), momentumBasedControllerFactory.get(), threadDataSynchronizer, simulationOutputWriter, humanoidGlobalDataProducer.get(), yoVariableServer, gravity.get(), robotModel.get().getEstimatorDT()); }
robotModel.getEstimatorDT()); DRCEstimatorThread estimatorThread = new DRCEstimatorThread(robotModel.getSimpleRobotName(), robotModel.getSensorInformation(), robotModel.getContactPointParameters(), robotModel, robotModel.getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, realtimeRos2Node, null, null, robotVisualizer,
private void setupLidarController() { DRCRobotLidarParameters lidarParameters = robotModel.get().getSensorInformation().getLidarParameters(0); if (lidarParameters != null && lidarParameters.getLidarSpindleJointName() != null) { PIDLidarTorqueController pidLidarTorqueController = new PIDLidarTorqueController(humanoidFloatingRootJointRobot, lidarParameters.getLidarSpindleJointName(), lidarParameters.getLidarSpindleVelocity(), robotModel.get().getSimulateDT()); humanoidFloatingRootJointRobot.setController(pidLidarTorqueController); } }
private void setupLidarController() { DRCRobotLidarParameters lidarParameters = robotModel.get().getSensorInformation().getLidarParameters(0); if (lidarParameters != null && lidarParameters.getLidarSpindleJointName() != null) { PIDLidarTorqueController pidLidarTorqueController = new PIDLidarTorqueController(humanoidFloatingRootJointRobot, lidarParameters.getLidarSpindleJointName(), lidarParameters.getLidarSpindleVelocity(), robotModel.get().getSimulateDT()); humanoidFloatingRootJointRobot.setController(pidLidarTorqueController); } }
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(); }
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); } }
HighLevelControllerParameters highLevelControllerParameters = robotModel.getHighLevelControllerParameters(); DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation(); SideDependentList<String> feetForceSensorNames = sensorInformation.getFeetForceSensorNames(); SideDependentList<String> feetContactSensorNames = sensorInformation.getFeetContactSensorNames();
private void createControllerCore(YoVariableRegistry walkingControllerRegistry) { JointBasics[] jointsToIgnore = DRCControllerThread.createListOfJointsToIgnore(fullRobotModel, robotModel, robotModel.getSensorInformation()); JointBasics[] jointsToOptimizeFor = HighLevelHumanoidControllerToolbox.computeJointsToOptimizeFor(fullRobotModel, jointsToIgnore); FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); ReferenceFrame centerOfMassFrame = referenceFrames.getCenterOfMassFrame(); WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters(); MomentumOptimizationSettings momentumOptimizationSettings = walkingControllerParameters.getMomentumOptimizationSettings(); WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, gravityZ, rootJoint, jointsToOptimizeFor, centerOfMassFrame, momentumOptimizationSettings, yoGraphicsListRegistry, walkingControllerRegistry); toolbox.setupForInverseDynamicsSolver(contactableBodies); JointPrivilegedConfigurationParameters jointPrivilegedConfigurationParameters = walkingControllerParameters.getJointPrivilegedConfigurationParameters(); toolbox.setJointPrivilegedConfigurationParameters(jointPrivilegedConfigurationParameters); FeedbackControlCommandList template = managerFactory.createFeedbackControlTemplate(); controllerOutput = new JointDesiredOutputList(fullRobotModel.getControllableOneDoFJoints()); controllerCore = new WholeBodyControllerCore(toolbox, template, controllerOutput, walkingControllerRegistry); }
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, logModelProvider, params.isBehaviorVisualizerEnabled(), sensorInformation, params.getTimeToWaitBeforeStartingDiagnostics()); } else { new IHMCHumanoidBehaviorManager(robotModel, logModelProvider, params.isBehaviorVisualizerEnabled(), sensorInformation); } PacketCommunicator behaviorModuleCommunicator = PacketCommunicator .createIntraprocessPacketCommunicator(NetworkPorts.BEHAVIOUR_MODULE_PORT, NET_CLASS_LIST); packetRouter.attachPacketCommunicator(PacketDestination.BEHAVIOR_MODULE, behaviorModuleCommunicator); behaviorModuleCommunicator.connect(); String methodName = "setupBehaviorModule "; printModuleConnectedDebugStatement(PacketDestination.BEHAVIOR_MODULE, methodName); } }
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 LocalObjectCommunicator createSimulatedSensorsPacketCommunicator() { scsSensorOutputPacketCommunicator = new LocalObjectCommunicator(); if (createSCSSimulatedSensors) { DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation(); DRCRobotJointMap jointMap = robotModel.getJointMap(); TimestampProvider timeStampProvider = avatarSimulation.getSimulatedRobotTimeProvider(); HumanoidFloatingRootJointRobot robot = avatarSimulation.getHumanoidFloatingRootJointRobot(); Graphics3DAdapter graphics3dAdapter = simulationConstructionSet.getGraphics3dAdapter(); printIfDebug("Streaming SCS Video"); DRCRobotCameraParameters cameraParameters = sensorInformation.getCameraParameters(0); if (cameraParameters != null) { String cameraName = cameraParameters.getSensorNameInSdf(); int width = sdfRobot.getCameraMount(cameraName).getImageWidth(); int height = sdfRobot.getCameraMount(cameraName).getImageHeight(); CameraConfiguration cameraConfiguration = new CameraConfiguration(cameraName); cameraConfiguration.setCameraMount(cameraName); int framesPerSecond = 25; DRCRenderedSceneVideoHandler drcRenderedSceneVideoHandler = new DRCRenderedSceneVideoHandler(scsSensorOutputPacketCommunicator); simulationConstructionSet.startStreamingVideoData(cameraConfiguration, width, height, drcRenderedSceneVideoHandler, timeStampProvider, framesPerSecond); } for (DRCRobotLidarParameters lidarParams : sensorInformation.getLidarParameters()) { DRCLidar.setupDRCRobotLidar(robot, graphics3dAdapter, scsSensorOutputPacketCommunicator, jointMap, lidarParams, timeStampProvider, true); } } return scsSensorOutputPacketCommunicator; }
DRCRobotSensorInformation robotSensorInfo = robotModel.getSensorInformation(); FlatGroundEnvironment flatGround = new FlatGroundEnvironment(); DRCNetworkModuleParameters networkModuleParameters = new DRCNetworkModuleParameters();
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()); }
DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation(); DRCRobotJointMap jointMap = robotModel.getJointMap(); TimestampProvider timeStampProvider = avatarSimulation.getSimulatedRobotTimeProvider();