private long updateTimer(YoDouble currentTimer, long currentTimerStartTime) { long endTime = System.nanoTime(); currentTimer.set(Conversions.nanosecondsToSeconds(endTime - currentTimerStartTime)); return endTime; }
private void benchmarkFunction(String name, int elements, DoubleSupplier f) { System.out.println("--- " + name + " ---"); long start = System.nanoTime(); double compressionFactor = f.getAsDouble(); long duration = System.nanoTime() - start; System.out.println("Compression factor: " + compressionFactor); System.out.println("Duration: " + Conversions.nanosecondsToSeconds(duration) + "s"); System.out.println("Time per data line: " + Conversions.nanosecondsToMilliseconds((double) (duration / elements)) + "ms"); System.out.println(); }
private void manageWalkChanged(boolean nowWalking) { if(nowWalking) lastStartWalkTime = Conversions.nanosecondsToSeconds(nanosecondstime.getLongValue()); else internalPriorWalkingDuration = walking_time.getDoubleValue(); }
public void run(Runnable command, String timeReportPrefix) { if (reportTimeEnabled.get()) { StopWatch stopWatch = stopWatchLocal.get(); stopWatch.reset(); stopWatch.start(); command.run(); long nanoTime = stopWatch.getNanoTime(); if (nanoTime > minimumNanoTimeToReport.get()) LogTools.info(timeReportPrefix + Conversions.nanosecondsToSeconds(nanoTime)); } else command.run(); } }
@Override public void receivedFrameAtTime(long hardwareTime, long pts, long timeScaleNumerator, long timeScaleDenumerator) { if(circularLongMap.size() > 0) { if(frame % 60 == 0) { System.out.println("[Decklink " + decklink + "] Received frame " + frame + " at time " + hardwareTime + "ns, delay: " + Conversions.nanosecondsToSeconds(circularLongMap.getLatestKey() - hardwareTime) + "s. pts: " + pts); } long robotTimestamp = circularLongMap.getValue(true, hardwareTime); try { if(frame == 0) { timestampWriter.write(timeScaleNumerator + "\n"); timestampWriter.write(timeScaleDenumerator + "\n"); } timestampWriter.write(robotTimestamp + " " + pts + "\n"); } catch (IOException e) { e.printStackTrace(); } ++frame; } }
@Override public void read(long currentClockTime) { long timestamp; if (threadDataSynchronizer != null) { timestamp = threadDataSynchronizer.getTimestamp(); handControllerTime.set(Conversions.nanosecondsToSeconds(timestamp)); } else { handControllerTime.add(Conversions.nanosecondsToSeconds(controlDTInNS)); } if (jointAngleProducer != null) { jointAngleProducer.sendHandJointAnglesPacket(); } }
@Override public void read(long currentClockTime) { long timestamp; if (threadDataSynchronizer != null) { timestamp = threadDataSynchronizer.getTimestamp(); handControllerTime.set(Conversions.nanosecondsToSeconds(timestamp)); } else { handControllerTime.add(Conversions.nanosecondsToSeconds(controlDTInNS)); } if (jointAngleProducer != null) { jointAngleProducer.sendHandJointAnglesPacket(); } }
@Override public void receivedTimestampAndData(long timestamp) { if(++counter % displayOneInNPackets == 0) { long delay = Conversions.nanosecondsToMilliseconds(lastTimestamp - timestamp); delayValue.setText(delayFormat.format(delay)); for (int i = 0; i < jointUpdaters.size(); i++) { jointUpdaters.get(i).update(); } scs.setTime(Conversions.nanosecondsToSeconds(timestamp)); updateLocalVariables(); scs.tickAndUpdate(); } }
private void updateRobotState() { robotDataReceiver.updateRobotModel(); long simTimestamp = robotDataReceiver.getSimTimestamp(); if (simTimestamp < 0) return; double currentTimeInSeconds = Conversions.nanosecondsToSeconds(simTimestamp); yoTime.set(currentTimeInSeconds); }
public boolean readAndProcessALogLineReturnTrueIfDone(double DT) { try { if (!readLogLine()) { return true; } timestamp.set(logLongArray.get()); robotTime.set(Conversions.nanosecondsToSeconds(timestamp.getLongValue() - initialTimestamp)); for (int i = 0; i < variablesToUpdate.length; i++) { YoVariable<?> variable = variablesToUpdate[i]; AtomicInteger position = indexes.get(variable); variable.setValueFromLongBits(logLongArray.get(position.get()), false); } } catch (IOException e) { e.printStackTrace(); } return false; }
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(); }
@Override public void doControl(long timestamp) { super.doControl(timestamp); //add additional tau for(int i = 0; i < joints.size(); i++) { OneDoFJoint joint = joints.get(i); if(joint.getName().equals(funcGenJoint.getEnumValue().getSdfName())) { joint.setTau(funcGen.getValue(Conversions.nanosecondsToSeconds(timestamp))+joint.getTau()); } } }
@Override public void notifyOfIndexChange(int newIndex) { if(!init_complete) { initTime = Conversions.nanosecondsToSeconds(nanosecondstime.getLongValue()); init_complete = true; } if(expo_isWalking.getBooleanValue()) walking_time.set(Conversions.nanosecondsToSeconds(nanosecondstime.getLongValue())-lastStartWalkTime+internalPriorWalkingDuration); total_time.set(Conversions.nanosecondsToSeconds(nanosecondstime.getLongValue())-initTime+startTime.getDoubleValue()); walking_time_value.setText(displayTime((int)walking_time.getDoubleValue()+(int)priorWalkingDuration.getDoubleValue())); operating_time_value.setText(displayTime((int)total_time.getDoubleValue())); stepCount_value.setText(String.format("%d",(int)(walking_time.getDoubleValue()*48.0/60.0)+startingStepCount.getLongValue())); distance_value.setText(String.format("%.1f",distanceTraveled.getDoubleValue()+startDistance.getDoubleValue())); battery_value.setText(String.format("%.1f%%",batteryPercentage(batteryLevel.getDoubleValue()))); avg_power_value.setText(String.format("%.1f",avgpower.getDoubleValue())); avg_vel_value.setText(String.format("%.2f",averageVelocity.getDoubleValue())); COT_value.setText(String.format("%.1f",COT.getDoubleValue())); }
private void setupCorrector() { externalPelvisPoseCreator = new ExternalPelvisPoseCreator(); PelvisPoseHistoryCorrection pelvisCorrector = new PelvisPoseHistoryCorrection(sixDofPelvisJoint, Conversions.nanosecondsToSeconds(1000000), registry, 100, externalPelvisPoseCreator); PelvisPoseHistoryCorrectorController robotController = new PelvisPoseHistoryCorrectorController(pelvisCorrector, simulationConstructionSet); robot.setController(robotController); setPelvisPoseHistoryCorrectorAlphaBreakFreq(registry, 0.015 , 0.015); }
public NormalOcTreeVisualizer() { Point3D lidarPosition = new Point3D(0.0, 0.0, 2.0); // callUpdateNode(); // callInsertPointCloud(); createPlane(lidarPosition, 12.0, 0.0, -0.05); // createBowl(0.5, new Point3D()); System.out.println("Number of leafs: " + ocTree.getNumberOfLeafNodes()); System.out.println("Initialized octree"); System.out.println("Computing normals"); long startTime = System.nanoTime(); ocTree.update(new ScanCollection(pointcloud, lidarPosition)); ocTree.update(new ScanCollection(pointcloud, lidarPosition)); ocTree.update(new ScanCollection(pointcloud, lidarPosition)); ocTree.update(new ScanCollection(pointcloud, lidarPosition)); ocTree.update(new ScanCollection(pointcloud, lidarPosition)); ocTree.update(new ScanCollection(pointcloud, lidarPosition)); long endTime = System.nanoTime(); System.out.println("Done computing normals: time it took = " + Conversions.nanosecondsToSeconds(endTime - startTime)); }
@Override public void doControl() { long startTime = System.nanoTime(); lowLevelOutputWriter.writeBefore(startTime); sensorReader.read(); humanoidReferenceFrames.updateFrames(); if (firstControlTick) { stateEstimator.initialize(); forceSensorStateUpdater.initialize(); automatedDiagnosticAnalysisController.initialize(); firstControlTick = false; } else { stateEstimator.doControl(); forceSensorStateUpdater.updateForceSensorState(); automatedDiagnosticAnalysisController.doControl(); } lowLevelOutputWriter.writeAfter(); long endTime = System.nanoTime(); controllerTime.set(Conversions.nanosecondsToSeconds(endTime - startTime)); averageControllerTime.update(); }
public void stopMeasurement() { final long currentNanoTime = System.nanoTime(); if ((currentNanoTime - timeOfFirstMeasurement) > measurementDelay) { final double timeTaken = Conversions.nanosecondsToSeconds(currentNanoTime - startTime); final double previousAverage = average.getDoubleValue(); double previousSumOfSquares = MathTools.square(standardDeviation.getDoubleValue()) * ((double) count.getLongValue()); count.increment(); current.set(timeTaken); average.set(previousAverage + (timeTaken - previousAverage) / ((double) count.getLongValue())); movingAverage.update(); double sumOfSquares = previousSumOfSquares + (timeTaken - previousAverage) * (timeTaken - average.getDoubleValue()); standardDeviation.set(Math.sqrt(sumOfSquares / ((double) count.getLongValue()))); if (timeTaken > maximum.getDoubleValue()) { maximum.set(timeTaken); } } }
int historicSample = sampleIndex(index + 1); this.robotTime[currentSample] = Conversions.nanosecondsToSeconds(timestamp); this.totalWork[currentSample] = totalWorkVariable.getDoubleValue();
long solutionTime = tf - t0; timeStatistics.addValue(Conversions.nanosecondsToSeconds(solutionTime)); iterationStatistics.addValue(calculator.getNumberOfIterations());
yoTime.set(Conversions.nanosecondsToSeconds(time - startTime));