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 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 void setAlphaFilterBreakFrequency(double breakFrequency) { correlationAlpha.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequency, dt)); }
public void setBreakFrequency(double breakFrequencyHertz, double dt) { this.alpha.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequencyHertz, dt)); }
public void setTaskspaceVelocityFromErrorFilterBreakFrequency(double breakFrequency) { alphaSpatialVelocityFromError.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequency, controlDT)); }
public void setMagnitudeFilterBreakFrequency(double breakFrequency) { magnitudeAlpha.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequency, dt)); }
public void setFilterBreakFrequencyForBaseFrameUpdater(double breakFrequency) { alphaBaseParentJointPose.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequency, controlDT)); }
/** * Get the desired alpha value, based on the break frequency given by the breakFrequencyProvider * * The value gets cached, and checked based on the current value of the breakFrequencyProvider. * This will be safe to rewind. * * @return alpha variable based on the value of breakFrequencyProvider and dt */ @Override public double getValue() { double currentBreakFrequency = breakFrequencyProvider.getValue(); if (currentBreakFrequency != previousBreakFrequency) { alpha = AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(currentBreakFrequency, dt); previousBreakFrequency = currentBreakFrequency; } return alpha; }
@Override public void variableChanged(YoVariable<?> v) { double alpha = AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(interpolationTranslationAlphaFilterBreakFrequency.getDoubleValue(), estimatorDT); interpolationTranslationAlphaFilter.setAlpha(alpha); } });
/** * Create an alpha filter given a name and a break frequency (in Hertz) that will be registered in the {@code SensorProcessing}'s {@code YoVariableRegistry}. * @param name name of the variable. * @param breakFrequency break frequency in Hertz * @return a {@code DoubleYoVariable} to be used when adding a low-pass filter stage using the methods in this class such as {@link SensorProcessing#addJointVelocityAlphaFilter(DoubleYoVariable, boolean)}. */ public DoubleYoVariable createAlphaFilter(String name, double breakFrequency) { DoubleYoVariable alphaFilter = new DoubleYoVariable(name, registry); alphaFilter.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequency, updateDT)); return alphaFilter; }
@Override public void variableChanged(YoVariable<?> v) { double freq = anglularVelocityFilterBreakFrequeny.getDoubleValue(); double alpha = AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(freq, controllerDt); yoAngularVelocityAlphaFilter.set(alpha); } });
@Override public void variableChanged(YoVariable<?> v) { double alpha = AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(interpolationRotationAlphaFilterBreakFrequency.getDoubleValue(), estimatorDT); interpolationRotationAlphaFilter.setAlpha(alpha); } });
@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); } } }
public void configureModuleParameters(boolean enableCompensation, boolean integrateRate, double delayBeforeTrustingFoot, double footLinearVelocityThreshold, double driftFilterFrequency, double driftRateFilterFrequency) { this.enableCompensation.set(enableCompensation); this.integrateDriftRate.set(integrateRate); this.delayBeforeTrustingFoot.set(delayBeforeTrustingFoot); this.yawDriftAlphaFilter.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(driftFilterFrequency, this.estimatorDT)); this.yawDriftRateAlphaFilter.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(driftRateFilterFrequency, this.estimatorDT)); this.footLinearVelocityThreshold.set(footLinearVelocityThreshold); }
@Override public void variableChanged(YoVariable<?> v) { alphaFilter_AlphaValue.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(alphaFilter_BreakFrequency.getDoubleValue(), dt.getDoubleValue())); } });
public void submitJointLimitEnforcementMethodCommand(JointLimitEnforcementMethodCommand command) { for (int idx = 0; idx < command.getNumberOfJoints(); idx++) { OneDoFJoint joint = command.getJoint(idx); jointLimitTypes.put(joint, command.getJointLimitReductionFactor(idx)); JointLimitParameters params = command.getJointLimitParameters(idx); if (params != null) { jointLimitParameters.get(joint).set(params); filterAlphas.get(joint).set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(params.getJointLimitFilterBreakFrequency(), controlDT)); velocityGains.get(joint).set(params.getVelocityControlGain()); } } }
public void configureModuleParameters(StateEstimatorParameters stateEstimatorParameters) { this.enableCompensation.set(stateEstimatorParameters.enableIMUYawDriftCompensation()); this.integrateDriftRate.set(stateEstimatorParameters.integrateEstimatedIMUYawDriftRate()); this.delayBeforeTrustingFoot.set(stateEstimatorParameters.getIMUYawDriftEstimatorDelayBeforeTrustingFoot()); this.footLinearVelocityThreshold.set(stateEstimatorParameters.getIMUYawDriftFootLinearVelocityThreshold()); double driftFilterFrequency = stateEstimatorParameters.getIMUYawDriftFilterFreqInHertz(); double driftRateFilterFrequency = stateEstimatorParameters.getIMUYawDriftRateFilterFreqInHertz(); this.yawDriftAlphaFilter.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(driftFilterFrequency, this.estimatorDT)); this.yawDriftRateAlphaFilter.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(driftRateFilterFrequency, this.estimatorDT)); }
public void configureModuleParameters(StateEstimatorParameters stateEstimatorParameters) { enableIMUBiasCompensation.set(stateEstimatorParameters.enableIMUBiasCompensation()); double biasFilterBreakFrequency = stateEstimatorParameters.getIMUBiasFilterFreqInHertz(); biasAlphaFilter.set(computeAlphaGivenBreakFrequencyProperly(biasFilterBreakFrequency, updateDT)); imuBiasEstimationThreshold.set(stateEstimatorParameters.getIMUBiasVelocityThreshold()); }
public TaskSpaceStiffnessCalculator(String namePrefix, double controlDT, YoVariableRegistry registry) { alphaLowPass = new DoubleYoVariable(namePrefix + "Alpha", registry); alphaLowPass.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(lowPassCutoffFreq_Hz, controlDT)); yoForcePointPosition = new YoFramePoint(namePrefix + "Position", world, registry); yoForcePointForce = new YoFrameVector(namePrefix + "Force", world, registry); yoForcePointVelocity = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector(namePrefix + "Velocity", "", alphaLowPass, controlDT, registry, yoForcePointPosition); yoForcePointForceRateOfChange = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector(namePrefix + "ForceRateOfChange", "", alphaLowPass, controlDT, registry, yoForcePointForce); yoForceAlongDirectionOfMotion = new DoubleYoVariable(namePrefix + "ForceAlongDirOfMotion", registry); yoForceRateOfChangeAlongDirectionOfMotion = new DoubleYoVariable(namePrefix + "DeltaForceAlongDirOfMotion", registry); yoStiffnessAlongDirectionOfMotion = new DoubleYoVariable(namePrefix + "StiffnessAlongDirOfMotion", registry); yoMaxStiffness = new DoubleYoVariable(namePrefix + "MaxStiffness", registry); yoCrossProductOfCurrentVelWithForce = new YoFrameVector(namePrefix + "VelocityCrossForce", world, registry); yoDirectionOfFreeMotion = new YoFrameVector(namePrefix + "DirOfFreeMotion", world, registry); addSimulatedSensorNoise = new BooleanYoVariable(namePrefix + "AddSimulatedNoise", registry); addSimulatedSensorNoise.set(false); }
public TaskSpaceStiffnessCalculator(String namePrefix, double controlDT, YoVariableRegistry registry) { alphaLowPass = new YoDouble(namePrefix + "Alpha", registry); alphaLowPass.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(lowPassCutoffFreq_Hz, controlDT)); yoForcePointPosition = new YoFramePoint3D(namePrefix + "Position", world, registry); yoForcePointForce = new YoFrameVector3D(namePrefix + "Force", world, registry); yoForcePointVelocity = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector(namePrefix + "Velocity", "", alphaLowPass, controlDT, registry, yoForcePointPosition); yoForcePointForceRateOfChange = FilteredVelocityYoFrameVector.createFilteredVelocityYoFrameVector(namePrefix + "ForceRateOfChange", "", alphaLowPass, controlDT, registry, yoForcePointForce); yoForceAlongDirectionOfMotion = new YoDouble(namePrefix + "ForceAlongDirOfMotion", registry); yoForceRateOfChangeAlongDirectionOfMotion = new YoDouble(namePrefix + "DeltaForceAlongDirOfMotion", registry); yoStiffnessAlongDirectionOfMotion = new YoDouble(namePrefix + "StiffnessAlongDirOfMotion", registry); yoMaxStiffness = new YoDouble(namePrefix + "MaxStiffness", registry); yoCrossProductOfCurrentVelWithForce = new YoFrameVector3D(namePrefix + "VelocityCrossForce", world, registry); yoDirectionOfFreeMotion = new YoFrameVector3D(namePrefix + "DirOfFreeMotion", world, registry); addSimulatedSensorNoise = new YoBoolean(namePrefix + "AddSimulatedNoise", registry); addSimulatedSensorNoise.set(false); }