@Override public PeriodicThreadScheduler createPeriodicThreadScheduler(String name) { return new PeriodicNonRealtimeThreadScheduler(name); }
@Override public PeriodicThreadScheduler createPeriodicThreadScheduler(String name) { return new PeriodicNonRealtimeThreadScheduler(name); }
protected void startYoVariableServer() { if (!startYoVariableServer) return; PeriodicThreadScheduler scheduler = new PeriodicNonRealtimeThreadScheduler("WholeBodyIKScheduler"); yoVariableServer = new YoVariableServer(getClass(), scheduler, modelProvider, LogSettings.TOOLBOX, YO_VARIABLE_SERVER_DT); yoVariableServer.setMainRegistry(registry, fullRobotModel, yoGraphicsListRegistry); startYoVariableServerOnAThread(yoVariableServer); yoVariableServerScheduled = executorService.scheduleAtFixedRate(createYoVariableServerRunnable(yoVariableServer), 0, updatePeriodMilliseconds, TimeUnit.MILLISECONDS); }
private void setupYoVariableServer() { if (robotModel.get().getLogSettings().isLog()) { yoVariableServer = new YoVariableServer(getClass(), new PeriodicNonRealtimeThreadScheduler("DRCSimulationYoVariableServer"), robotModel.get().getLogModelProvider(), robotModel.get().getLogSettings(), robotModel.get().getEstimatorDT()); } else { yoVariableServer = null; } }
public static PlanarRegionBipedalFootstepPlannerVisualizer createWithYoVariableServer(double dtForViz, FullRobotModel fullRobotModel, LogModelProvider logModelProvider, SideDependentList<ConvexPolygon2d> footPolygonsInSoleFrame, String namePrefix) { YoVariableRegistry registry = new YoVariableRegistry(PlanarRegionBipedalFootstepPlannerVisualizerFactory.class.getSimpleName()); YoGraphicsListRegistry graphicsListRegistry = new YoGraphicsListRegistry(); PlanarRegionBipedalFootstepPlannerVisualizer footstepPlannerVisualizer = new PlanarRegionBipedalFootstepPlannerVisualizer(10, footPolygonsInSoleFrame, registry, graphicsListRegistry); PeriodicThreadScheduler scheduler = new PeriodicNonRealtimeThreadScheduler("PlannerScheduler"); YoVariableServer yoVariableServer = new YoVariableServer(namePrefix + PlanarRegionBipedalFootstepPlannerVisualizerFactory.class.getSimpleName(), scheduler, logModelProvider, LogSettings.FOOTSTEP_PLANNER, dtForViz); yoVariableServer.setSendKeepAlive(true); footstepPlannerVisualizer.setTickAndUpdatable(yoVariableServer); yoVariableServer.setMainRegistry(registry, fullRobotModel, graphicsListRegistry); yoVariableServer.start(); return footstepPlannerVisualizer; }
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); } }
PeriodicThreadScheduler scheduler = new PeriodicNonRealtimeThreadScheduler("WholeBodyIKScheduler"); yoVariableServer = new YoVariableServer(getClass(), scheduler, modelProvider, LogSettings.KINEMATICS_TOOLBOX, YO_VARIABLE_SERVER_DT); yoVariableServer.setMainRegistry(registry, kinematicsToolBoxController.getDesiredFullRobotModel(), yoGraphicsListRegistry);
private void createPoseCommunicator() { if (useNetworking.get()) { JointConfigurationGatherer jointConfigurationGathererAndProducer = new JointConfigurationGatherer(fullRobotModel.get()); PeriodicThreadScheduler scheduler = new PeriodicNonRealtimeThreadScheduler("PoseCommunicator"); poseCommunicator = new DRCPoseCommunicator(fullRobotModel.get(), jointConfigurationGathererAndProducer, null, globalDataProducer, timestampProvider.get(), sensorReader.getSensorRawOutputMapReadOnly(), controllerManager.getMotionStatusHolder(), null, scheduler, netClassList.get()); } else { poseCommunicator = null; } }
YoVariableServer yoVariableServer = new YoVariableServer(getClass(), new PeriodicNonRealtimeThreadScheduler("GazeboYoVariableServer"), robotModel.getLogModelProvider(), robotModel.getLogSettings(), robotModel.getEstimatorDT()); robotModel.getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer, new PeriodicNonRealtimeThreadScheduler("DRCPoseCommunicator"), dataProducer, yoVariableServer, gravity); estimatorThread.setExternalPelvisCorrectorSubscriber(externalPelvisPoseSubscriber); DRCControllerThread controllerThread = new DRCControllerThread(robotModel, robotModel.getSensorInformation(), controllerFactory, threadDataSynchronizer,
PeriodicThreadScheduler scheduler = new PeriodicNonRealtimeThreadScheduler("BehaviorScheduler"); yoVariableServer = new YoVariableServer(getClass(), scheduler, modelProvider, LogSettings.BEHAVIOR, BEHAVIOR_YO_VARIABLE_SERVER_DT);
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; } }
controllerFactory.setICPOptimizationControllerParameters(icpOptimizationParameters); if (setupControllerNetworkSubscriber) controllerFactory.createControllerNetworkSubscriber(new PeriodicNonRealtimeThreadScheduler("CapturabilityBasedStatusProducer"), controllerPacketCommunicator);
RobotMotionStatusHolder robotMotionStatusFromController = new RobotMotionStatusHolder(); DRCRobotSensorInformation sensorInformation = drcRobotModel.getSensorInformation(); PeriodicNonRealtimeThreadScheduler scheduler = new PeriodicNonRealtimeThreadScheduler(threadName); final DRCPoseCommunicator poseCommunicator = new DRCPoseCommunicator(humanoidFullRobotModel, jointConfigurationGatherer, auxiliaryRobotDataProvider, dataProducer, sensorOutputMapReadOnly, sensorRawOutputMapReadOnly,
controllerFactory.setICPOptimizationControllerParameters(icpOptimizationParameters); if (setupControllerNetworkSubscriber) controllerFactory.createControllerNetworkSubscriber(new PeriodicNonRealtimeThreadScheduler("CapturabilityBasedStatusProducer"), controllerPacketCommunicator);
sensorReaderFactory, threadDataSynchronizer, new PeriodicNonRealtimeThreadScheduler("DRCSimGazeboYoVariableServer"), globalDataProducer, yoVariableServer, gravity);
publisherTopicNameGenerator, realtimeRos2Node, sensorOutputMapReadOnly, sensorRawOutputMapReadOnly, robotMotionStatusFromController, sensorInformation); PeriodicNonRealtimeThreadScheduler scheduler2 = new PeriodicNonRealtimeThreadScheduler(threadName); scheduler2.schedule(new Runnable()
yoVariableServer = new YoVariableServer(getClass(), new PeriodicNonRealtimeThreadScheduler("DRCSimulationYoVariableServer"), drcRobotModel.getLogModelProvider(), drcRobotModel.getLogSettings(), drcRobotModel.getEstimatorDT());