public DrillDetectionCalibrationHelper(){ int numberOfBands = (int) ((upperFrequencyBound - lowerFrequencyBound) / frequencyBandRange); double upperFrequency; double lowerFrequency; for (int i = 0; i < numberOfBands; i++){ upperFrequency = upperFrequencyBound - i*frequencyBandRange; lowerFrequency = upperFrequency - frequencyBandRange; rawBandMagnitudes.add(new DoubleYoVariable("raw"+(int)lowerFrequency+""+(int)upperFrequency+"BandMagnitude", registry)); filteredBandMagnitudes.add(new AlphaFilteredYoVariable("filtered"+(int)lowerFrequency+""+(int)upperFrequency+"BandMagnitude", registry, 0.9)); } }
public void setAlphaFilterBreakFrequency(double breakFrequency) { correlationAlpha.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequency, dt)); }
@Override public void update(double currentLength) { x = currentLength*springProperties.getDirectionallity(); s.set(Math.signum(last_x-x)*0.5+0.5); s_filt.update(); F = isSpringEngaged() ? -k_L*(x-x0_L)*s_filt.getDoubleValue() -k_U*(x-x0_U)*(1-s_filt.getDoubleValue()) : 0; last_x = x; }
public static void main(String[] args) { double dt = 1 / 1e3; for (double i = 2; i < 1.0/dt; i = i * 1.2) { double alpha = computeAlphaGivenBreakFrequency(i, dt); double alphaProperly = computeAlphaGivenBreakFrequencyProperly(i, dt); System.out.println("freq=" + i + ", alpha=" + alpha + ", alphaProperly=" + alphaProperly); } System.out.println(computeBreakFrequencyGivenAlpha(0.8, 0.006)); System.out.println(computeAlphaGivenBreakFrequencyProperly(20, 0.006)); System.out.println(computeAlphaGivenBreakFrequencyProperly(20, 0.003)); }
public AlphaFilteredYoVariable(String name, String description, YoVariableRegistry registry, DoubleProvider alphaVariable, YoDouble positionVariable) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", description, registry); this.position = positionVariable; if (alphaVariable == null) alphaVariable = createAlphaYoDouble(name, 0.0, registry); this.alphaVariable = alphaVariable; reset(); }
public SpringJointOutputWriter(FloatingRootJointRobot robot, SimulatedElasticityParameters elasticityParameters, double dt) { ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint simulatedJoint : oneDegreeOfFreedomJoints) { if(elasticityParameters.isSpringJoint(simulatedJoint)) { elasticJoints.add(simulatedJoint); String jointName = simulatedJoint.getName(); double breakFrequency = AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(20.0, dt); AlphaFilteredYoVariable filteredDesired = new AlphaFilteredYoVariable(jointName + "_filteredQDesired", registry, breakFrequency); filteredDesired.update(0.0); filteredDesiredJointAngles.put(simulatedJoint, filteredDesired); YoDouble stiffness = new YoDouble(simulatedJoint.getName() + "_stiffness", registry); stiffness.set(elasticityParameters.getStiffness(simulatedJoint)); jointStiffness.put(simulatedJoint, stiffness); YoDouble maxDeflection = new YoDouble(simulatedJoint.getName() + "_maxDeflection", registry); maxDeflection.set(elasticityParameters.getMaxDeflection(simulatedJoint)); maxDeflections.put(simulatedJoint, maxDeflection); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testNoisyFixedPosition() { // Use a reasonably large alpha for a reasonably large amount of noise double alpha = 0.8; YoVariableRegistry registry = new YoVariableRegistry("testRegistry"); YoDouble positionVariable = new YoDouble("positionVariable", registry); AlphaFilteredYoVariable alphaFilteredYoVariable = new AlphaFilteredYoVariable("alphaFilteredYoVariable", registry, alpha, positionVariable); double pseudoNoise = 0; positionVariable.set(10); for (int i = 0; i < 10000; i++) { // Oscillate the position about some uniformly distributed fixed point slightly larger than 10 if (i % 2 == 0) { pseudoNoise = rng.nextDouble(); } positionVariable.add(Math.pow(-1, i) * pseudoNoise); alphaFilteredYoVariable.update(); } assertEquals(10, alphaFilteredYoVariable.getDoubleValue(), 1); }
heightCorrectedFilteredForSingularityAvoidance.reset(); heightVelocityCorrectedFilteredForSingularityAvoidance.reset(); heightAcceleretionCorrectedFilteredForSingularityAvoidance.reset(); heightCorrectedFilteredForSingularityAvoidance.update(desiredCenterOfMassHeightPoint.getZ()); heightVelocityCorrectedFilteredForSingularityAvoidance.update(comHeightDataToCorrect.getComHeightVelocity()); heightAcceleretionCorrectedFilteredForSingularityAvoidance.update(comHeightDataToCorrect.getComHeightAcceleration()); heightCorrectedFilteredForSingularityAvoidance.update(desiredCenterOfMassHeightPoint.getZ()); heightVelocityCorrectedFilteredForSingularityAvoidance.set(comHeightDataToCorrect.getComHeightVelocity()); heightAcceleretionCorrectedFilteredForSingularityAvoidance.set(comHeightDataToCorrect.getComHeightAcceleration()); heightCorrectedFilteredForSingularityAvoidance.getDoubleValue()); comHeightDataToCorrect.setComHeightVelocity(heightVelocityCorrectedFilteredForSingularityAvoidance.getDoubleValue()); comHeightDataToCorrect.setComHeightAcceleration(heightAcceleretionCorrectedFilteredForSingularityAvoidance.getDoubleValue()); if (Math.abs(desiredCenterOfMassHeightPoint.getZ() - heightCorrectedFilteredForSingularityAvoidance.getDoubleValue()) <= 5e-3) heightCorrectedFilteredForSingularityAvoidance.update(desiredCenterOfMassHeightPoint.getZ()); comHeightDataToCorrect.setComHeight(desiredCenterOfMassHeightPoint.getReferenceFrame(), heightCorrectedFilteredForSingularityAvoidance.getDoubleValue()); equivalentDesiredHipVelocity.scale(comXYVelocity.getX() / equivalentDesiredHipVelocity.getX()); equivalentDesiredHipVelocity.changeFrame(worldFrame); heightVelocityCorrectedFilteredForSingularityAvoidance.update(equivalentDesiredHipVelocity.getZ()); comHeightDataToCorrect.setComHeightVelocity(heightVelocityCorrectedFilteredForSingularityAvoidance.getDoubleValue()); heightAcceleretionCorrectedFilteredForSingularityAvoidance.update(equivalentDesiredHipPitchAcceleration.getZ()); comHeightDataToCorrect.setComHeightAcceleration(heightAcceleretionCorrectedFilteredForSingularityAvoidance.getDoubleValue());
unachievedSwingTranslationFiltered.update(unachievedSwingTranslation.getZ()); desiredCenterOfMassHeightPoint.setZ(desiredCenterOfMassHeightPoint.getZ() + unachievedSwingTranslationFiltered.getDoubleValue()); if (USE_UNREACHABLE_FOOTSTEP_CORRECTION_ON_POSITION) comHeightDataToCorrect.setComHeight(worldFrame, desiredCenterOfMassHeightPoint.getZ()); unachievedSwingTranslationFiltered.set(0.0); unachievedSwingVelocityFiltered.update(unachievedSwingVelocity.getZ()); comHeightDataToCorrect.setComHeightVelocity(comHeightDataToCorrect.getComHeightVelocity() + unachievedSwingVelocityFiltered.getDoubleValue()); unachievedSwingVelocityFiltered.set(0.0); unachievedSwingAccelerationFiltered.update(unachievedSwingAcceleration.getZ()); comHeightDataToCorrect .setComHeightAcceleration(comHeightDataToCorrect.getComHeightAcceleration() + unachievedSwingAccelerationFiltered.getDoubleValue()); unachievedSwingAccelerationFiltered.set(0.0);
final YoDouble lowerHeadPitchPercentage = new YoDouble("SliderLowerHeadPitchPercentage", registry); final YoDouble upperHeadPitchPercentage = new YoDouble("SliderUpperHeadPitchPercentage", registry); final AlphaFilteredYoVariable alphaFilteredHeadYawPercentage = new AlphaFilteredYoVariable("AlphaFilteredHeadYawPercentage",registry,alpha,headYawPercentage); final AlphaFilteredYoVariable alphaFilteredUpperHeadPitchPercentage = new AlphaFilteredYoVariable("AlphaFilteredUpperHeadPitchPercentage",registry,alpha,upperHeadPitchPercentage); final AlphaFilteredYoVariable alphaFilteredLowerHeadPitchYawPercentage = new AlphaFilteredYoVariable("AlphaFilteredLowerHeadPitchPercentage",registry,alpha,lowerHeadPitchPercentage); final LinkedHashMap<NeckJointName, ImmutablePair<Double, Double>> sliderBoardControlledNeckJointsWithLimits = drcRobotModel .getSliderBoardParameters().getSliderBoardControlledNeckJointsWithLimits(); sliderBoardConfigurationManager.setKnob(1, headYawPercentage, 0.0, 1.0); headYawPercentage.set(standPrepPercentage); alphaFilteredHeadYawPercentage.set(headYawPercentage.getDoubleValue()); sliderBoardConfigurationManager.setSlider(++sliderNumber, lowerHeadPitchPercentage, 0.0, 1.0); lowerHeadPitchPercentage.set(standPrepPercentage); alphaFilteredLowerHeadPitchYawPercentage.set(lowerHeadPitchPercentage.getDoubleValue()); sliderBoardConfigurationManager.setSlider(++sliderNumber, upperHeadPitchPercentage, 0.0, 1.0); upperHeadPitchPercentage.set(standPrepPercentage); alphaFilteredUpperHeadPitchPercentage.set(upperHeadPitchPercentage.getDoubleValue());
public double getSensorZForceLowPassFilteredInWorld() { return lowPassSensorForceZ.getDoubleValue(); }
public void update(double currentPosition) { if (!hasBeenCalled.getBooleanValue()) { hasBeenCalled.set(true); set(currentPosition); } if (alphaVariable == null) { set(alpha * getDoubleValue() + (1.0 - alpha) * currentPosition); } else { set(alphaVariable.getDoubleValue() * getDoubleValue() + (1.0 - alphaVariable.getDoubleValue()) * currentPosition); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testAlphaAndBreakFrequencyComputations() { for (int i = 0; i < 1000; i++) { double dt = rng.nextDouble(); double expectedAlpha = rng.nextDouble(); double breakFrequency = AlphaFilteredYoVariable.computeBreakFrequencyGivenAlpha(expectedAlpha, dt); double actualAlpha = AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequency, dt); assertEquals(expectedAlpha, actualAlpha, 1e-10); double maxFrequency = 0.5 * 0.5 / dt; double expectedBreakFrequency = maxFrequency * rng.nextDouble(); double alpha = AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(expectedBreakFrequency, dt); double actualBreakFrequency = AlphaFilteredYoVariable.computeBreakFrequencyGivenAlpha(alpha, dt); assertEquals(expectedBreakFrequency, actualBreakFrequency, 1e-7); } } }
@Override public void variableChanged(YoVariable<?> v) { double alpha = AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(interpolationTranslationAlphaFilterBreakFrequency.getDoubleValue(), estimatorDT); interpolationTranslationAlphaFilter.setAlpha(alpha); } });
private void initializeAlphaFilter(double alphaFilterPosition) { alphaFilter_PositionValue.set(alphaFilterPosition); alphaFilter.set(0.0); previousClippedAlphaFilterValue.set(0.0); cLippedAlphaFilterValue.set(0.0); hasBeenCalled.set(false); }
@Override public double getBreakFrequencyForSpineJointVelocityEstimation() { return AlphaFilteredYoVariable.computeBreakFrequencyGivenAlpha(0.95, 0.002); }
public SpringJointOutputWriter(FloatingRootJointRobot robot, SimulatedElasticityParameters elasticityParameters, double dt) { ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint simulatedJoint : oneDegreeOfFreedomJoints) { if(elasticityParameters.isSpringJoint(simulatedJoint)) { elasticJoints.add(simulatedJoint); String jointName = simulatedJoint.getName(); double breakFrequency = AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(20.0, dt); AlphaFilteredYoVariable filteredDesired = new AlphaFilteredYoVariable(jointName + "_filteredQDesired", registry, breakFrequency); filteredDesired.update(0.0); filteredDesiredJointAngles.put(simulatedJoint, filteredDesired); DoubleYoVariable stiffness = new DoubleYoVariable(simulatedJoint.getName() + "_stiffness", registry); stiffness.set(elasticityParameters.getStiffness(simulatedJoint)); jointStiffness.put(simulatedJoint, stiffness); DoubleYoVariable maxDeflection = new DoubleYoVariable(simulatedJoint.getName() + "_maxDeflection", registry); maxDeflection.set(elasticityParameters.getMaxDeflection(simulatedJoint)); maxDeflections.put(simulatedJoint, maxDeflection); } } }