From f1a2d76edafb8c37e8f168cfa9abc731568a5502 Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Mon, 14 Oct 2024 13:21:53 -0500 Subject: [PATCH] moved filtered parameters and pd gains to higher level projects removed unused visualizer types --- ...celerationLimitedYoVariableVisualizer.java | 10 +- .../ButterworthFusedYoVariableVisualizer.java | 225 ------------------ ...ngularVelocityYoFrameVectorVisualizer.java | 4 +- ...irstOrderFilteredYoVariableVisualizer.java | 24 +- ...eResponseFilteredYoVariableVisualizer.java | 223 ----------------- .../JerkLimitedYoVariableVisualizer.java | 6 +- ...entationTrajectoryGeneratorVisualizer.java | 6 +- 7 files changed, 28 insertions(+), 470 deletions(-) delete mode 100644 src/main/java/us/ihmc/scsVisualizers/filters/ButterworthFusedYoVariableVisualizer.java delete mode 100644 src/main/java/us/ihmc/scsVisualizers/filters/InfiniteImpulseResponseFilteredYoVariableVisualizer.java diff --git a/src/main/java/us/ihmc/scsVisualizers/filters/AccelerationLimitedYoVariableVisualizer.java b/src/main/java/us/ihmc/scsVisualizers/filters/AccelerationLimitedYoVariableVisualizer.java index e6b8fa2..2c068f1 100644 --- a/src/main/java/us/ihmc/scsVisualizers/filters/AccelerationLimitedYoVariableVisualizer.java +++ b/src/main/java/us/ihmc/scsVisualizers/filters/AccelerationLimitedYoVariableVisualizer.java @@ -3,7 +3,7 @@ import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.robotics.math.filters.AccelerationLimitedYoVariable; -import us.ihmc.robotics.math.filters.FilteredVelocityYoVariable; +import us.ihmc.robotics.math.filters.FilteredFiniteDifferenceYoVariable; import us.ihmc.robotics.math.functionGenerator.YoFunctionGenerator; import us.ihmc.robotics.math.functionGenerator.YoFunctionGeneratorMode; import us.ihmc.simulationconstructionset.Robot; @@ -24,8 +24,8 @@ public class AccelerationLimitedYoVariableVisualizer private final YoDouble raw; private final YoDouble alphaVariable; - private final FilteredVelocityYoVariable rawRate; - private final FilteredVelocityYoVariable rawAcceleration; + private final FilteredFiniteDifferenceYoVariable rawRate; + private final FilteredFiniteDifferenceYoVariable rawAcceleration; private static double amplitude = 2.0; private static double frequency = 0.1; @@ -43,8 +43,8 @@ public AccelerationLimitedYoVariableVisualizer() raw = new YoDouble("raw", registry); alphaVariable = new YoDouble("alpha", registry); alphaVariable.set(0.0); //set to zero to prevent smoothing of velocity - rawRate = new FilteredVelocityYoVariable("rawRate", "", alphaVariable, raw, dt, registry); - rawAcceleration = new FilteredVelocityYoVariable("rawAcceleration", "", alphaVariable, rawRate, dt, registry); + rawRate = new FilteredFiniteDifferenceYoVariable("rawRate", "", alphaVariable, raw, dt, registry); + rawAcceleration = new FilteredFiniteDifferenceYoVariable("rawAcceleration", "", alphaVariable, rawRate, dt, registry); scs = new SimulationConstructionSet(robot); scs.changeBufferSize(bufferSize); diff --git a/src/main/java/us/ihmc/scsVisualizers/filters/ButterworthFusedYoVariableVisualizer.java b/src/main/java/us/ihmc/scsVisualizers/filters/ButterworthFusedYoVariableVisualizer.java deleted file mode 100644 index 6056d2e..0000000 --- a/src/main/java/us/ihmc/scsVisualizers/filters/ButterworthFusedYoVariableVisualizer.java +++ /dev/null @@ -1,225 +0,0 @@ -package us.ihmc.scsVisualizers.filters; - -import us.ihmc.robotics.math.filters.AlphaFusedYoVariable; -import us.ihmc.robotics.math.filters.ButterworthFilteredYoVariable; -import us.ihmc.robotics.math.filters.ButterworthFilteredYoVariable.ButterworthFilterType; -import us.ihmc.robotics.math.filters.ButterworthFusedYoVariable; -import us.ihmc.simulationConstructionSetTools.util.inputdevices.MidiSliderBoard; -import us.ihmc.simulationconstructionset.GraphConfiguration; -import us.ihmc.simulationconstructionset.Robot; -import us.ihmc.simulationconstructionset.SimulationConstructionSet; -import us.ihmc.simulationconstructionset.util.RobotController; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; - -/** - *

Title:

- * - *

Description:

- * - *

Copyright: Copyright (c) 2006

- * - *

Company:

- * - * @author not attributable - * @version 1.0 - */ -public class ButterworthFusedYoVariableVisualizer -{ - private static final double SIMULATION_DT = 0.001; - private static final int RECORD_RATE = 1; - - private static final double MAX_FREQ_HERTZ = 1.0 / ((2.0 * Math.PI) * SIMULATION_DT); - - { - System.out.println("MAX_FREQ_HERTZ = " + MAX_FREQ_HERTZ); - } - - public static void main(String[] args) - { - new ButterworthFusedYoVariableVisualizer(); - } - - public ButterworthFusedYoVariableVisualizer() - { - Robot robot = new Robot("null") - { - } ; - - ButterworthFusedYoVariableController controller = new ButterworthFusedYoVariableController((YoDouble)robot.findVariable("t")); - robot.setController(controller); - - SimulationConstructionSet scs = new SimulationConstructionSet(robot); - - scs.setDT(SIMULATION_DT, RECORD_RATE); - - GraphConfiguration autoscale = new GraphConfiguration("autoscale", GraphConfiguration.AUTO_SCALING); - GraphConfiguration tipsOf3db = new GraphConfiguration("tipsOf3db", GraphConfiguration.MANUAL_SCALING, 0.70, 0.72); - - GraphConfiguration[] graphConfigurations = new GraphConfiguration[] {autoscale, tipsOf3db}; - scs.setupGraphConfigurations(graphConfigurations); - - scs.setupGraphGroup("testFused", new String[][][] - { - { - {"original", "fused"}, {"autoscale"} - }, - { - {"original", "alphaFused"}, {"autoscale"} - }, - { - {"original", "lowPass"}, {"autoscale"} - }, - { - {"original", "highPass"}, {"autoscale"} - }, - { - {"original", "lowPassPlusHighPass"}, {"autoscale"} - }, - { - {"alpha", "cutoffFreq"}, {"autoscale"} - } - }); - - scs.setupGraphGroup("lowAndHigh", new String[][][] - { - { - {"lowPass", "highPass"}, {"tipsOf3db"} - }, - }); - - - - scs.setupEntryBoxGroup("testFused", new String[] {"alpha", "amp1", "freq1", "amp2", "freq2"}); - - - scs.setupConfiguration("testFused", "all", "testFused", "testFused"); - - scs.selectConfiguration("testFused"); - - scs.hideViewport(); - -// scs.createNewGraphWindow("lowAndHigh", 1, true); - - MidiSliderBoard evolutionUC33E = new MidiSliderBoard(scs); - evolutionUC33E.setSlider(1, "alpha", scs, 0.0, 1.0); - evolutionUC33E.setSlider(2, "amp1", scs, 0.0, 1.0); - evolutionUC33E.setSlider(3, "freq1", scs, 7.95, 9.0); - evolutionUC33E.setSlider(4, "amp2", scs, 0.0, 1.0); - evolutionUC33E.setSlider(5, "freq2", scs, 0, 100); - - - Thread thread = new Thread(scs); - thread.start(); - - while (true) - { - try - { - Thread.sleep(4000); - } - catch (InterruptedException ex) - { - } - } - } - - - private class ButterworthFusedYoVariableController implements RobotController - { - private final YoRegistry registry = new YoRegistry("ButterworthFusedYoVariableController"); - - private final YoDouble dc = new YoDouble("dc", registry); - - private final YoDouble amp1 = new YoDouble("amp1", registry); - private final YoDouble amp2 = new YoDouble("amp2", registry); - private final YoDouble amp3 = new YoDouble("amp3", registry); - - private final YoDouble freq1 = new YoDouble("freq1", registry); - private final YoDouble freq2 = new YoDouble("freq2", registry); - private final YoDouble freq3 = new YoDouble("freq3", registry); - - private final YoDouble original = new YoDouble("original", registry); - - private final YoDouble highFreqNoise = new YoDouble("highFreqNoise", registry); - private final YoDouble lowFreqNoise = new YoDouble("lowFreqNoise", registry); - - private final YoDouble originalPlusHighFreqNoise = new YoDouble("originalPlusHighFreqNoise", registry); - private final YoDouble originalPlusLowFreqNoise = new YoDouble("originalPlusLowFreqNoise", registry); - - private final YoDouble lowPassPlusHighPass = new YoDouble("lowPassPlusHighPass", registry); - - private final YoDouble alpha = new YoDouble("alpha", registry); - private final YoDouble cutoffFreq = new YoDouble("cutoffFreq", registry); - - private final ButterworthFilteredYoVariable lowPass, highPass; - - private final ButterworthFusedYoVariable fused; - private final AlphaFusedYoVariable alphaFused; - - private final YoDouble t; - - public ButterworthFusedYoVariableController(YoDouble t) - { - this.t = t; - - fused = new ButterworthFusedYoVariable("fused", registry, alpha, originalPlusHighFreqNoise, originalPlusLowFreqNoise); - alphaFused = new AlphaFusedYoVariable("alphaFused", registry, alpha, originalPlusHighFreqNoise, originalPlusLowFreqNoise); - - lowPass = new ButterworthFilteredYoVariable("lowPass", registry, alpha, originalPlusHighFreqNoise, ButterworthFilterType.LOW_PASS); - highPass = new ButterworthFilteredYoVariable("highPass", registry, alpha, originalPlusLowFreqNoise, ButterworthFilterType.HIGH_PASS); - - amp1.set(1.0); - freq1.set(10.0); - - amp2.set(0.1); - freq2.set(100.0); - - amp3.set(0.2); - freq3.set(3.0); - - dc.set(1.2); - - alpha.set(0.75); - } - - public YoRegistry getYoRegistry() - { - return registry; - } - - public void doControl() - { - cutoffFreq.set((1.0 - alpha.getDoubleValue()) / (2.0 * Math.PI * SIMULATION_DT)); - - - original.set(dc.getDoubleValue() + amp1.getDoubleValue() * Math.cos(2.0 * Math.PI * freq1.getDoubleValue() * t.getDoubleValue()) + amp2.getDoubleValue() * Math.cos(2.0 * Math.PI * freq2.getDoubleValue() * t.getDoubleValue()) - + amp3.getDoubleValue() * Math.signum(Math.cos(2.0 * Math.PI * freq3.getDoubleValue() * t.getDoubleValue()))); - - originalPlusHighFreqNoise.set(original.getDoubleValue() + highFreqNoise.getDoubleValue()); - originalPlusLowFreqNoise.set(original.getDoubleValue() + lowFreqNoise.getDoubleValue()); - - lowPass.update(); - highPass.update(); - fused.update(); - alphaFused.update(); - - lowPassPlusHighPass.set(lowPass.getDoubleValue() + highPass.getDoubleValue()); - } - - public String getName() - { - return "butterworthFusedYoVariableController"; - } - - public void initialize() - { - } - - public String getDescription() - { - return getName(); - } - - } -} diff --git a/src/main/java/us/ihmc/scsVisualizers/filters/FiniteDifferenceAngularVelocityYoFrameVectorVisualizer.java b/src/main/java/us/ihmc/scsVisualizers/filters/FiniteDifferenceAngularVelocityYoFrameVectorVisualizer.java index d3db504..32cd6ea 100644 --- a/src/main/java/us/ihmc/scsVisualizers/filters/FiniteDifferenceAngularVelocityYoFrameVectorVisualizer.java +++ b/src/main/java/us/ihmc/scsVisualizers/filters/FiniteDifferenceAngularVelocityYoFrameVectorVisualizer.java @@ -2,7 +2,7 @@ import us.ihmc.euclid.referenceFrame.FrameQuaternion; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.robotics.math.filters.FiniteDifferenceAngularVelocityYoFrameVector; +import us.ihmc.robotics.math.filters.FiniteDifferenceAngularVelocityYoFrameVector3D; import us.ihmc.robotics.math.trajectories.SimpleOrientationTrajectoryGenerator; import us.ihmc.simulationconstructionset.Robot; import us.ihmc.simulationconstructionset.SimulationConstructionSet; @@ -27,7 +27,7 @@ public FiniteDifferenceAngularVelocityYoFrameVectorVisualizer() traj.initialize(); YoFrameQuaternion orientationToDifferentiate = new YoFrameQuaternion("orientationToDifferentiate", worldFrame, registry); - FiniteDifferenceAngularVelocityYoFrameVector filteredAngularVelocityYoFrameVector = new FiniteDifferenceAngularVelocityYoFrameVector("angularVelocityFD", orientationToDifferentiate, dt, registry); + FiniteDifferenceAngularVelocityYoFrameVector3D filteredAngularVelocityYoFrameVector = new FiniteDifferenceAngularVelocityYoFrameVector3D("angularVelocityFD", orientationToDifferentiate, dt, registry); Robot robot = new Robot("Dummy"); YoDouble yoTime = robot.getYoTime(); diff --git a/src/main/java/us/ihmc/scsVisualizers/filters/FirstOrderFilteredYoVariableVisualizer.java b/src/main/java/us/ihmc/scsVisualizers/filters/FirstOrderFilteredYoVariableVisualizer.java index 499baf6..fdeaa45 100644 --- a/src/main/java/us/ihmc/scsVisualizers/filters/FirstOrderFilteredYoVariableVisualizer.java +++ b/src/main/java/us/ihmc/scsVisualizers/filters/FirstOrderFilteredYoVariableVisualizer.java @@ -1,8 +1,8 @@ package us.ihmc.scsVisualizers.filters; -import us.ihmc.robotics.math.filters.FirstOrderBandPassFilteredYoVariable; -import us.ihmc.robotics.math.filters.FirstOrderFilteredYoVariable; -import us.ihmc.robotics.math.filters.FirstOrderFilteredYoVariable.FirstOrderFilterType; +import us.ihmc.robotics.math.filters.FirstOrderBandPassFilteredYoDouble; +import us.ihmc.robotics.math.filters.FirstOrderFilteredYoDouble; +import us.ihmc.robotics.math.filters.FirstOrderFilteredYoDouble.FirstOrderFilterType; import us.ihmc.simulationConstructionSetTools.util.inputdevices.MidiSliderBoard; import us.ihmc.simulationconstructionset.Robot; import us.ihmc.simulationconstructionset.SimulationConstructionSet; @@ -56,9 +56,9 @@ private static class FilterController implements RobotController private final YoDouble minPassThroughFreq_radPerSec; private final YoDouble maxPassThroughFreq_radPerSec; - private final FirstOrderFilteredYoVariable highPass; - private final FirstOrderFilteredYoVariable lowPass; - private final FirstOrderBandPassFilteredYoVariable bandPass; + private final FirstOrderFilteredYoDouble highPass; + private final FirstOrderFilteredYoDouble lowPass; + private final FirstOrderBandPassFilteredYoDouble bandPass; private final YoDouble properAmplitudeHighPass; private final YoDouble properAmplitudeLowPass; @@ -87,9 +87,15 @@ public FilterController(Robot robot, double DT, String name) this.maxPassThroughFreq_radPerSec = new YoDouble("maxFreq_radPerSec", registry); maxPassThroughFreq_radPerSec.set(100.0); - this.highPass = new FirstOrderFilteredYoVariable("highPass", "", minPassThroughFreq_radPerSec.getDoubleValue() / (2.0*Math.PI), robot.getYoTime(), FirstOrderFilterType.HIGH_PASS, registry); - this.lowPass = new FirstOrderFilteredYoVariable("lowPass", "", maxPassThroughFreq_radPerSec.getDoubleValue() / (2.0*Math.PI), robot.getYoTime(), FirstOrderFilterType.LOW_PASS, registry); - this.bandPass = new FirstOrderBandPassFilteredYoVariable("bandPass", "", minPassThroughFreq_radPerSec.getDoubleValue() / (2.0*Math.PI), maxPassThroughFreq_radPerSec.getDoubleValue() / (2.0*Math.PI), robot.getYoTime(), registry); + this.highPass = new FirstOrderFilteredYoDouble("highPass", "", minPassThroughFreq_radPerSec.getDoubleValue() / (2.0 * Math.PI), robot.getYoTime(), FirstOrderFilterType.HIGH_PASS, registry); + this.lowPass = new FirstOrderFilteredYoDouble("lowPass", "", maxPassThroughFreq_radPerSec.getDoubleValue() / (2.0 * Math.PI), robot.getYoTime(), FirstOrderFilterType.LOW_PASS, registry); + this.bandPass = new FirstOrderBandPassFilteredYoDouble("bandPass", + "", + minPassThroughFreq_radPerSec.getDoubleValue() / (2.0*Math.PI), + maxPassThroughFreq_radPerSec.getDoubleValue() / (2.0*Math.PI), + robot.getYoTime(), + FirstOrderBandPassFilteredYoDouble.FirstOrderFilterType.BAND, + registry); this.properAmplitudeHighPass = new YoDouble("properAmplitudeHighPass", registry); this.properAmplitudeLowPass = new YoDouble("properAmplitudeLowPass", registry); diff --git a/src/main/java/us/ihmc/scsVisualizers/filters/InfiniteImpulseResponseFilteredYoVariableVisualizer.java b/src/main/java/us/ihmc/scsVisualizers/filters/InfiniteImpulseResponseFilteredYoVariableVisualizer.java deleted file mode 100644 index f7c66cd..0000000 --- a/src/main/java/us/ihmc/scsVisualizers/filters/InfiniteImpulseResponseFilteredYoVariableVisualizer.java +++ /dev/null @@ -1,223 +0,0 @@ -package us.ihmc.scsVisualizers.filters; - -import us.ihmc.robotics.dataStructures.ComplexNumber; -import us.ihmc.robotics.math.filters.AlphaFilteredYoVariable; -import us.ihmc.robotics.math.filters.ButterworthFilteredYoVariable; -import us.ihmc.robotics.math.filters.ButterworthFilteredYoVariable.ButterworthFilterType; -import us.ihmc.robotics.math.filters.InfiniteImpulseResponseFilteredYoVariable; -import us.ihmc.robotics.math.filters.RateLimitedYoVariable; -import us.ihmc.simulationconstructionset.Robot; -import us.ihmc.simulationconstructionset.SimulationConstructionSet; -import us.ihmc.simulationconstructionset.SimulationConstructionSetParameters; -import us.ihmc.simulationconstructionset.util.RobotController; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; - -/** - *

Title:

- * - *

Description:

- * - *

Copyright: Copyright (c) 2006

- * - *

Company:

- * - * @author not attributable - * @version 1.0 - */ -public class InfiniteImpulseResponseFilteredYoVariableVisualizer -{ - public static void main(String[] args) - { - final double DT = 0.0025; - final int simulationTicksPerControlTick = 1; - - Robot nullRobot = new Robot("Nullbot"); - - TestIIRFilterController testIIRFilterController = new TestIIRFilterController(nullRobot, DT, "testIIRFilterController"); - nullRobot.setController(testIIRFilterController, simulationTicksPerControlTick); - - SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(); - parameters.setDataBufferSize(16000); - SimulationConstructionSet scs = new SimulationConstructionSet(nullRobot, parameters); - - scs.setDT(DT, 1); - - scs.setupGraphGroup("IIR Filter", new String[][] { { "input", "alphaFilterOutput", "alphaFilterTestOutput" }, - { "input", "butterworthOneFilterOutput", "butterworthOneFilterTestOutput" }, { "input", "highPass", "highPassInverse" } }); - - scs.setupGraphGroup("Nonlinear Filter", new String[][] { { "input", "highPassInverse" }, { "highPass" }, { "input", "doubleFiltered" }, - { "input", "lowPass" } }); - - // scs.createNewGraphWindow("IIR Filter", 1, true); - scs.createNewGraphWindow("Nonlinear Filter", 1, true); - - Thread thread = new Thread(scs); - thread.start(); - - } - - private static class TestIIRFilterController implements RobotController - { - private final YoRegistry registry = new YoRegistry("TestIIRFilterController"); - - private final YoDouble input = new YoDouble("input", registry); - - private final YoDouble chirp = new YoDouble("chirp", registry); - private final YoDouble square = new YoDouble("square", registry); - private final YoDouble oneHertz = new YoDouble("oneHertz", registry); - private final YoDouble threeHertz = new YoDouble("threeHertz", registry); - private final YoDouble sixHertz = new YoDouble("sixHertz", registry); - private final YoDouble twelveHertz = new YoDouble("twelveHertz", registry); - - private final YoDouble t; - - private final AlphaFilteredYoVariable alphaFilterTestOutput = new AlphaFilteredYoVariable("alphaFilterTestOutput", registry, 0.95, input); - private final ButterworthFilteredYoVariable butterworthOneFilterTestOutput = new ButterworthFilteredYoVariable("butterworthOneFilterTestOutput", - registry, 0.95, input, ButterworthFilterType.LOW_PASS); - private final InfiniteImpulseResponseFilteredYoVariable alphaFilterOutput = new InfiniteImpulseResponseFilteredYoVariable("alphaFilterOutput", 1, 0, - registry); - private final InfiniteImpulseResponseFilteredYoVariable butterworthOneFilterOutput = new InfiniteImpulseResponseFilteredYoVariable( - "butterworthOneFilterOutput", 1, 1, registry); - - private final InfiniteImpulseResponseFilteredYoVariable lowPass = new InfiniteImpulseResponseFilteredYoVariable("lowPass", 4, 4, registry); - - private final InfiniteImpulseResponseFilteredYoVariable highPass = new InfiniteImpulseResponseFilteredYoVariable("highPass", 2, 2, registry); - private final InfiniteImpulseResponseFilteredYoVariable highPassInverse = new InfiniteImpulseResponseFilteredYoVariable("highPassInverse", 2, 2, registry); - private final YoDouble saturation = new YoDouble("saturation", registry); - private final YoDouble amplitude = new YoDouble("amplitude", registry); - - private final YoDouble alphaButterworth = new YoDouble("alphaButterworth", registry); - private final ButterworthFilteredYoVariable doubleFilteredOutput = new ButterworthFilteredYoVariable("doubleFiltered", registry, alphaButterworth, - highPassInverse, ButterworthFilterType.LOW_PASS); - - private final YoDouble maxRate; - private final RateLimitedYoVariable rateLimitFilterd; - - private String name; - - public TestIIRFilterController(Robot robot, double DT, String name) - { - this.name = name; - maxRate = new YoDouble("maxRate", registry); - rateLimitFilterd = new RateLimitedYoVariable("rateLimitFilter", registry, maxRate, input, DT); - - maxRate.set(0.5); - - this.t = (YoDouble) robot.findVariable("t"); - - double alpha = 0.95; - - alphaFilterOutput.setPolesAndZeros(1.0 - alpha, new double[] { alpha }, null, null, null); - butterworthOneFilterOutput.setPolesAndZeros(0.5 * (1.0 - alpha), new double[] { alpha }, null, new double[] { -1.0 }, null); - - // double alphaOne = 0.9; - // double alphaTwo = 0.98; - - // highPass.setPolesAndZeros(1.0, null, new double[]{alphaOne, alphaOne, alphaOne, alphaOne}, null, new double[]{alphaTwo, alphaTwo, alphaTwo, alphaTwo}); - // highPassInverse.setPolesAndZeros(1.0, null, new double[]{alphaTwo, alphaTwo, alphaTwo, alphaTwo}, null, new double[]{alphaOne, alphaOne, alphaOne, alphaOne}); - - // double a1 = 0.99, b1 = 0.02; // Low Frequency - // double a1 = 0.975, b1 = 0.0; // Low Frequency - double a1 = 0.98, b1 = 0.02; // Low Frequency - - // double a1 = 0.98, b1 = 0.03; // Low Frequency - // double a1 = 0.99, b1 = 0.0; // Low Frequency - - // double a2 = 0.8, b2 = 0.2; // High Frequency - double a2 = 0.85, b2 = 0.15; // High Frequency - - // double a2 = 0.2, b2 = 0.0; // High Frequency - // double a2 = 0.6, b2 = 0.4; // High Frequency - // double a2 = 0.90, b2 = 0.05; // High Frequency - // double a2 = 0.96, b2 = 0.05; // - - ComplexNumber[] complexZeroPairs = new ComplexNumber[] { new ComplexNumber(a1, b1) }; // Low frequency. - ComplexNumber[] complexPolePairs = new ComplexNumber[] { new ComplexNumber(a2, b2) }; // High frequency. - - double gainAtZero = (1.0 - 2.0 * a1 + a1 * a1 + b1 * b1) / (1.0 - 2.0 * a2 + a2 * a2 + b2 * b2); - - highPass.setPolesAndZeros(1.0 / gainAtZero, null, complexPolePairs, null, complexZeroPairs); - highPassInverse.setPolesAndZeros(gainAtZero, null, complexZeroPairs, null, complexPolePairs); - - // double a1LowPass = 0.92, b1LowPass = 0.08; // Low Frequency - double a1LowPass = 0.92, b1LowPass = 0.04; // Low Frequency - ComplexNumber[] lowPassComplexPolPairs = new ComplexNumber[] { new ComplexNumber(a1LowPass, b1LowPass), new ComplexNumber(a1LowPass, b1LowPass) }; // Low frequency. - lowPass.setPolesAndZeros(null, lowPassComplexPolPairs, new double[] { -1.0, -1.0, -1.0, -1.0 }, null); - - saturation.set(0.3); - amplitude.set(0.3); - - alphaButterworth.set(0.9); - } - - public void doControl() - { - double time = t.getDoubleValue() % 5.0; - chirp.set(amplitude.getDoubleValue() * Math.sin(4.0 * 2.0 * Math.PI * time * time)); - - oneHertz.set(amplitude.getDoubleValue() * Math.sin(2.0 * Math.PI * 1.0 * t.getDoubleValue())); - threeHertz.set(amplitude.getDoubleValue() * Math.sin(2.0 * Math.PI * 3.0 * t.getDoubleValue())); - sixHertz.set(amplitude.getDoubleValue() * Math.sin(2.0 * Math.PI * 6.0 * t.getDoubleValue())); - twelveHertz.set(amplitude.getDoubleValue() * Math.sin(2.0 * Math.PI * 12.0 * t.getDoubleValue())); - - square.set(amplitude.getDoubleValue() * Math.sin(2.0 * Math.PI * 1.0 * time)); - if (square.getDoubleValue() > 0.0) - square.set(amplitude.getDoubleValue()); - if (square.getDoubleValue() < 0.0) - square.set(amplitude.getDoubleValue()); - - if (t.getDoubleValue() < 2.0) - input.set(oneHertz.getDoubleValue()); - else if (t.getDoubleValue() < 4.0) - input.set(threeHertz.getDoubleValue()); - else if (t.getDoubleValue() < 6.0) - input.set(sixHertz.getDoubleValue()); - else if (t.getDoubleValue() < 8.0) - input.set(twelveHertz.getDoubleValue()); - else if (t.getDoubleValue() < 10.0) - input.set(square.getDoubleValue()); - else - input.set(chirp.getDoubleValue()); - - alphaFilterOutput.update(input.getDoubleValue()); - butterworthOneFilterOutput.update(input.getDoubleValue()); - - alphaFilterTestOutput.update(); - butterworthOneFilterTestOutput.update(); - - lowPass.update(input.getDoubleValue()); - highPass.update(input.getDoubleValue()); - - double saturatedHighPass = highPass.getDoubleValue(); - if (saturatedHighPass > saturation.getDoubleValue()) - saturatedHighPass = saturation.getDoubleValue(); - if (saturatedHighPass < -saturation.getDoubleValue()) - saturatedHighPass = -saturation.getDoubleValue(); - - highPassInverse.update(saturatedHighPass); - doubleFilteredOutput.update(); - - rateLimitFilterd.update(); - } - - public YoRegistry getYoRegistry() - { - return registry; - } - - public String getName() - { - return this.name; - } - - public void initialize() - { - } - - public String getDescription() - { - return getName(); - } - } -} diff --git a/src/main/java/us/ihmc/scsVisualizers/filters/JerkLimitedYoVariableVisualizer.java b/src/main/java/us/ihmc/scsVisualizers/filters/JerkLimitedYoVariableVisualizer.java index 060156d..673475c 100644 --- a/src/main/java/us/ihmc/scsVisualizers/filters/JerkLimitedYoVariableVisualizer.java +++ b/src/main/java/us/ihmc/scsVisualizers/filters/JerkLimitedYoVariableVisualizer.java @@ -2,7 +2,7 @@ import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.robotics.math.filters.JerkLimitedYoVariable; +import us.ihmc.robotics.math.filters.JerkLimitedYoDouble; import us.ihmc.robotics.math.functionGenerator.YoFunctionGenerator; import us.ihmc.robotics.math.functionGenerator.YoFunctionGeneratorMode; import us.ihmc.simulationconstructionset.Robot; @@ -24,8 +24,8 @@ public class JerkLimitedYoVariableVisualizer private final YoDouble inputAcceleration = new YoDouble("inputAcceleration", registry); double dt = 0.006; - JerkLimitedYoVariable smoothedYoVariable = new JerkLimitedYoVariable("smoothedPosition", registry, maxAcceleration, maxJerk, inputPosition, inputVelocity, - inputAcceleration, dt); + JerkLimitedYoDouble smoothedYoVariable = new JerkLimitedYoDouble("smoothedPosition", registry, maxAcceleration, maxJerk, inputPosition, inputVelocity, + inputAcceleration, dt); public static void main(String[] args) { diff --git a/src/main/java/us/ihmc/scsVisualizers/trajectories/HermiteCurveBasedOrientationTrajectoryGeneratorVisualizer.java b/src/main/java/us/ihmc/scsVisualizers/trajectories/HermiteCurveBasedOrientationTrajectoryGeneratorVisualizer.java index d766824..073e6e1 100644 --- a/src/main/java/us/ihmc/scsVisualizers/trajectories/HermiteCurveBasedOrientationTrajectoryGeneratorVisualizer.java +++ b/src/main/java/us/ihmc/scsVisualizers/trajectories/HermiteCurveBasedOrientationTrajectoryGeneratorVisualizer.java @@ -14,7 +14,7 @@ import us.ihmc.graphicsDescription.yoGraphics.YoGraphicVector; import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry; import us.ihmc.robotics.geometry.AngleTools; -import us.ihmc.robotics.math.filters.FiniteDifferenceAngularVelocityYoFrameVector; +import us.ihmc.robotics.math.filters.FiniteDifferenceAngularVelocityYoFrameVector3D; import us.ihmc.robotics.math.trajectories.HermiteCurveBasedOrientationTrajectoryGenerator; import us.ihmc.simulationconstructionset.Robot; import us.ihmc.simulationconstructionset.SimulationConstructionSet; @@ -45,8 +45,8 @@ public class HermiteCurveBasedOrientationTrajectoryGeneratorVisualizer private FrameVector3D angularVelocityPrev = new FrameVector3D(); double angle, omega; - private final FiniteDifferenceAngularVelocityYoFrameVector FD2AngularVelocity = new FiniteDifferenceAngularVelocityYoFrameVector("FD2AngularVelocity", - worldFrame, dt, registry); + private final FiniteDifferenceAngularVelocityYoFrameVector3D FD2AngularVelocity = new FiniteDifferenceAngularVelocityYoFrameVector3D("FD2AngularVelocity", + worldFrame, dt, registry); private final YoFrameVector3D FDAngularVelocity = new YoFrameVector3D("FDAngularVelocity", worldFrame, registry); private final YoDouble FDAngularVelocityMagnitude = new YoDouble("FDAngularVelocityMagnitude", registry);