@Override public boolean containsNaN() { return time.isNaN() || waypoint1d.containsNaN(); }
public void doControl() { long timestamp = timestampProvider.getTimestamp(); if (wakeUpTime.isNaN()) wakeUpTime.set(Conversions.nanosecondsToSeconds(timestamp)); yoTime.set(Conversions.nanosecondsToSeconds(timestamp) - wakeUpTime.getDoubleValue()); if (ValkyrieRosControlController.ENABLE_FINGER_JOINTS) fingerController.doControl(); updateCommandCalculators(); }
private void terminateToolboxController() { toolboxSolution.setDestination(PacketDestination.BEHAVIOR_MODULE.ordinal()); reportMessage(toolboxSolution); //nodePlotter.closeAll(); state.set(CWBToolboxState.DO_NOTHING); double totalTime = 0.0; if (!initialGuessComputationTime.isNaN()) totalTime += initialGuessComputationTime.getDoubleValue(); if (!treeExpansionComputationTime.isNaN()) totalTime += treeExpansionComputationTime.getDoubleValue(); if (!shortcutPathComputationTime.isNaN()) totalTime += shortcutPathComputationTime.getDoubleValue(); if (!motionGenerationComputationTime.isNaN()) totalTime += motionGenerationComputationTime.getDoubleValue(); totalComputationTime.set(totalTime); if (VERBOSE) { PrintTools.info("==========================================="); PrintTools.info("initialGuessComputationTime is " + initialGuessComputationTime.getDoubleValue()); PrintTools.info("treeExpansionComputationTime is " + treeExpansionComputationTime.getDoubleValue()); PrintTools.info("shortcutPathComputationTime is " + shortcutPathComputationTime.getDoubleValue()); PrintTools.info("motionGenerationComputationTime is " + motionGenerationComputationTime.getDoubleValue()); PrintTools.info("toolbox executing time is " + totalComputationTime.getDoubleValue() + " seconds " + currentNumberOfIterations.getIntegerValue()); PrintTools.info("==========================================="); } isDone.set(true); }