private long updateTimer(YoDouble currentTimer, long currentTimerStartTime) { long endTime = System.nanoTime(); currentTimer.set(Conversions.nanosecondsToSeconds(endTime - currentTimerStartTime)); return endTime; }
public SimulatedDRCRobotTimeProvider(double controlDT) { nanoSecondsPerTick = Conversions.secondsToNanoseconds(controlDT); }
public double getDurationSeconds() { return Conversions.millisecondsToSeconds(getDurationMillis()); }
@Override public void run() { if (Thread.interrupted()) return; serverTime += Conversions.millisecondsToSeconds(updatePeriodMilliseconds); yoVariableServer.update(Conversions.secondsToNanoseconds(serverTime)); } };
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(); }
robotTimestamp.add(Conversions.millisecondsToSeconds(1)); stateImpl.process(); System.out.println(state + ": " + Conversions.nanosecondsToSeconds(endTime/TEST_ITERATIONS) + "s/it. Min: " + Conversions.nanosecondsToSeconds(min) + "s ; max: " + Conversions.nanosecondsToSeconds(max) + "s") ;
error.set(targetTranslation.getX(), targetTranslation.getY(), targetTranslation.getZ(), targetYaw); long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(targets[i], timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); success &= simulateAndBlockAndCatchExceptions(Conversions.nanosecondsToSeconds(1000000) * 3); externalPelvisPoseCreator.setNewestPose(posePacket); success &= simulateAndBlockAndCatchExceptions(Conversions.nanosecondsToSeconds(1000000) * 3); success &= simulateAndBlockAndCatchExceptions(Conversions.nanosecondsToSeconds(1000000) * 100);
public void predict() { long startTime = System.nanoTime(); // State prediction. state.predict(); // Get linearized plant model and predict error covariance. state.getFMatrix(F); state.getQMatrix(Q); filterMatrixOps.predictErrorCovariance(Pprior, F, Pposterior, Q); predictionTime.set(Conversions.nanosecondsToMilliseconds((double) (System.nanoTime() - startTime))); }
private static int calculateTimeoutInMilliseconds(double testDuration) { double potentialTimeout = (testDuration * 5.0); double timeoutInSeconds = (potentialTimeout > 30.0) ? MathTools.roundToSignificantFigures(potentialTimeout, 2) : 30.0; return Conversions.secondsToMilliseconds(timeoutInSeconds); } }
public double getBuildDurationMinutes() { return Conversions.millisecondsToMinutes(Integer.valueOf(buildDuration)); }
long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(targets[i], timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); success &= simulateAndBlockAndCatchExceptions(Conversions.nanosecondsToSeconds(1000000) * 3); externalPelvisPoseCreator.setNewestPose(posePacket); success &= simulateAndBlockAndCatchExceptions(Conversions.nanosecondsToSeconds(1000000) * 3); success &= simulateAndBlockAndCatchExceptions(Conversions.nanosecondsToSeconds(1000000) * 100);
@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(); } }
@Override public void run() { if (Thread.interrupted()) return; serverTime += Conversions.millisecondsToSeconds(updatePeriodMilliseconds); yoVariableServer.update(Conversions.secondsToNanoseconds(serverTime)); } };
public void correct() { long startTime = System.nanoTime(); // From the sensor get the linearized measurement model and the measurement residual sensor.assembleFullJacobian(H, residual, robotState); // Compute the kalman gain and correct the state sensor.getRMatrix(R); filterMatrixOps.computeKalmanGain(K, Pprior, H, R); state.getStateVector(Xprior); filterMatrixOps.updateState(Xposterior, Xprior, K, residual); // Update the error covariance. filterMatrixOps.updateErrorCovariance(Pposterior, K, H, Pprior); // Update the state data structure after the correction step. state.setStateVector(Xposterior); correctionTime.set(Conversions.nanosecondsToMilliseconds((double) (System.nanoTime() - startTime))); }
@Override public void run() { ThreadTools.sleep((long) Conversions.secondsToMilliseconds(180)); timeNotElapsed.set(false); } };
public double getBuildDurationMinutes() { return Conversions.millisecondsToMinutes(Integer.valueOf(buildDuration)); }
private void manageWalkChanged(boolean nowWalking) { if(nowWalking) lastStartWalkTime = Conversions.nanosecondsToSeconds(nanosecondstime.getLongValue()); else internalPriorWalkingDuration = walking_time.getDoubleValue(); }
long timeStamp = Conversions.secondsToNanoseconds(simulationConstructionSet.getTime()); TimeStampedTransform3D timeStampedTransform = new TimeStampedTransform3D(targets[i], timeStamp); StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0); success &= simulateAndBlockAndCatchExceptions(Conversions.nanosecondsToSeconds(1000000) * 3); externalPelvisPoseCreator.setNewestPose(posePacket); success &= simulateAndBlockAndCatchExceptions(Conversions.nanosecondsToSeconds(1000000) * 3); success &= simulateAndBlockAndCatchExceptions(Conversions.nanosecondsToSeconds(1000000) * 100);
@Override public void tickAndUpdate(double timeToSetInSeconds) { this.update(Conversions.secondsToNanoseconds(timeToSetInSeconds)); }
actualControlDT.set(Conversions.nanosecondsToMilliseconds((double) (nanoTime - lastReadSystemTime))); lastReadSystemTime = nanoTime; controllerTime.set(Conversions.nanosecondsToSeconds(controllerTimestamp.getLongValue()));