public void update() { update(yoVariableToFilter.getDoubleValue()); }
public void update() { update(yoVariableToFilter.getDoubleValue()); }
@Override public void update() { referenceVelocity.update(); referenceVelocityFiltered.update(); filteredRawVelocityToCheck.update(); filteredProcessedVelocityToCheck.update(); if (!referenceVelocityFiltered.getHasBufferWindowFilled()) return; delayEstimatorProcessedPosition.update(); delayEstimatorRawVelocity.update(); delayEstimatorProcessedVelocity.update(); }
@Override public void update() { for (StepprJoint joint : StepprJoint.values) { double actuatorTau = 0; OneDoFJoint oneDoFJoint = wholeBodyControlJoints.get(joint); if (joint.isLinear()) { actuatorTau = oneDoFJoint.getTau() / joint.getRatio(); StepprActuator actuator = joint.getActuators()[0]; predictedMotorPower.get(actuator).set(calcPower(actuatorTau, actuator.getKm())); } } updateKnees(StepprJoint.LEFT_KNEE_Y, StepprActuator.LEFT_KNEE, StepprJoint.RIGHT_KNEE_Y, StepprActuator.RIGHT_KNEE); updateAnkle(rightAnkleCalculator, StepprJoint.RIGHT_ANKLE_X, StepprJoint.RIGHT_ANKLE_Y, StepprActuator.RIGHT_ANKLE_LEFT, StepprActuator.RIGHT_ANKLE_RIGHT); updateAnkle(leftAnkleCalculator, StepprJoint.LEFT_ANKLE_X, StepprJoint.LEFT_ANKLE_Y, StepprActuator.LEFT_ANKLE_LEFT, StepprActuator.LEFT_ANKLE_RIGHT); double sumTotalMotorPower = 0; for (StepprActuator actuator : StepprActuator.values) { sumTotalMotorPower += predictedMotorPower.get(actuator).getDoubleValue(); } totalPredictedRobotPower.set(sumTotalMotorPower); totalPredictedRobotPowerAverage.update(); }
@Override public void update() { for (WandererJoint joint : WandererJoint.values) { double actuatorTau = 0; OneDoFJoint oneDoFJoint = wholeBodyControlJoints.get(joint); if (joint.isLinear()) { actuatorTau = oneDoFJoint.getTau() / joint.getRatio(); WandererActuator actuator = joint.getActuators()[0]; predictedMotorPower.get(actuator).set(calcPower(actuatorTau, actuator.getKm())); } } updateKnees(WandererJoint.LEFT_KNEE_Y, WandererActuator.LEFT_KNEE, WandererJoint.RIGHT_KNEE_Y, WandererActuator.RIGHT_KNEE); updateAnkle(rightAnkleCalculator, WandererJoint.RIGHT_ANKLE_X, WandererJoint.RIGHT_ANKLE_Y, WandererActuator.RIGHT_ANKLE_LEFT, WandererActuator.RIGHT_ANKLE_RIGHT); updateAnkle(leftAnkleCalculator, WandererJoint.LEFT_ANKLE_X, WandererJoint.LEFT_ANKLE_Y, WandererActuator.LEFT_ANKLE_LEFT, WandererActuator.LEFT_ANKLE_RIGHT); double sumTotalMotorPower = 0; for (WandererActuator actuator : WandererActuator.values) { sumTotalMotorPower += predictedMotorPower.get(actuator).getDoubleValue(); } totalPredictedRobotPower.set(sumTotalMotorPower); totalPredictedRobotPowerAverage.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); } } }
public void stopMeasurement() { final long currentNanoTime = System.nanoTime(); if ((currentNanoTime - timeOfFirstMeasurement) > measurementDelay) { final double timeTaken = ((double) (currentNanoTime - startTime)) / 1e9; 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); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testWithFixedSizeDoubleArrays() throws Exception { for (int i = 0; i < 100; i++) { YoVariableRegistry registry = new YoVariableRegistry("Blop"); Random random = new Random(6541654L); int windowSize = RandomNumbers.nextInt(random, 1, 1000); SimpleMovingAverageFilteredYoVariable sma = new SimpleMovingAverageFilteredYoVariable("tested", windowSize, registry); double amplitude = 100.0; double[] randomArray = RandomNumbers.nextDoubleArray(random, windowSize, amplitude); double expected = 0.0; for (double val : randomArray) expected += val / windowSize; for (int j = 0; j < randomArray.length; j++) { assertFalse(sma.getHasBufferWindowFilled()); sma.update(randomArray[j]); } assertTrue(sma.getHasBufferWindowFilled()); assertEquals(expected, sma.getDoubleValue(), 1.0e-10); } } }