From 442a1434833ee084c3f8eda26e48954272176ea6 Mon Sep 17 00:00:00 2001 From: Operator Computer Date: Thu, 7 Sep 2017 16:06:05 -0500 Subject: [PATCH 01/47] Renamed subsprojects to kebab-case. Moved all src and test directories in their respective maven directory structure Remove ihmc-java-extensions. Move everything into ihmc-commons except for a few things that needed Euclid. Moved those into robotics-toolkit. Switched to use FrameQuaternion from Euclid. Changed FrameTuple2D to FrameTuple2DReadOnly where possible. Reducing usage of FrameTuple3D. Got rid of YoFrameQuaternion.get(QuaternionBasics). Got rid of YoFrameQuaternion.get(RotationMatrix). Switched to euclid 0.7.7.1. switched the rate limited yo frame vector to take in frame tuples so it can be used with frame points --- .../AccelerationLimitedYoFrameVector2d.java | 85 +++++++ .../AccelerationLimitedYoVariable.java | 157 ++++++++++++ .../filters/AlphaBetaFilteredYoVariable.java | 121 +++++++++ .../AlphaFilteredWrappingYoVariable.java | 138 ++++++++++ .../filters/AlphaFilteredYoFramePoint.java | 139 ++++++++++ .../filters/AlphaFilteredYoFramePoint2d.java | 102 ++++++++ .../AlphaFilteredYoFrameQuaternion.java | 95 +++++++ .../filters/AlphaFilteredYoFrameVector.java | 127 +++++++++ .../filters/AlphaFilteredYoFrameVector2d.java | 107 ++++++++ .../math/filters/AlphaFilteredYoVariable.java | 171 +++++++++++++ ...lashCompensatingVelocityYoFrameVector.java | 77 ++++++ ...acklashCompensatingVelocityYoVariable.java | 240 ++++++++++++++++++ .../filters/BacklashProcessingYoVariable.java | 155 +++++++++++ .../filters/BetaFilteredYoFramePoint2d.java | 85 +++++++ .../filters/BetaFilteredYoFrameVector2d.java | 75 ++++++ .../math/filters/BetaFilteredYoVariable.java | 105 ++++++++ .../math/filters/DeadzoneYoFrameVector.java | 72 ++++++ .../math/filters/DeadzoneYoVariable.java | 69 +++++ .../math/filters/DelayedYoBoolean.java | 68 +++++ .../math/filters/DelayedYoDouble.java | 54 ++++ .../math/filters/DeltaLimitedYoVariable.java | 69 +++++ .../FilteredDiscreteVelocityYoVariable.java | 201 +++++++++++++++ .../FilteredVelocityYoFrameVector.java | 118 +++++++++ .../FilteredVelocityYoFrameVector2d.java | 91 +++++++ .../filters/FilteredVelocityYoVariable.java | 179 +++++++++++++ ...ifferenceAngularVelocityYoFrameVector.java | 104 ++++++++ .../FirstOrderBandPassFilteredYoVariable.java | 68 +++++ .../filters/FirstOrderFilteredYoVariable.java | 157 ++++++++++++ .../math/filters/GlitchFilteredYoBoolean.java | 110 ++++++++ .../math/filters/GlitchFilteredYoInteger.java | 82 ++++++ .../math/filters/JerkLimitedYoVariable.java | 151 +++++++++++ .../math/filters/ProcessingYoVariable.java | 6 + .../RateLimitedYoFrameOrientation.java | 124 +++++++++ .../filters/RateLimitedYoFrameVector.java | 125 +++++++++ .../filters/RateLimitedYoFrameVector2d.java | 107 ++++++++ .../math/filters/RateLimitedYoVariable.java | 120 +++++++++ ...acklashCompensatingVelocityYoVariable.java | 213 ++++++++++++++++ .../SecondOrderFilteredYoFrameVector.java | 95 +++++++ ...condOrderFilteredYoVariableParameters.java | 36 +++ ...pleMovingAverageFilteredYoFrameVector.java | 95 +++++++ ...SimpleMovingAverageFilteredYoVariable.java | 91 +++++++ .../ihmc/robotics/math/frames/YoMatrix.java | 134 ++++++++++ 42 files changed, 4718 insertions(+) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBetaFilteredYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java new file mode 100644 index 00000000..c9a99b76 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java @@ -0,0 +1,85 @@ +package us.ihmc.robotics.math.filters; + + + +import us.ihmc.euclid.referenceFrame.FrameVector2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.robotics.math.frames.YoFrameVector2d; + +public class AccelerationLimitedYoFrameVector2d extends YoFrameVector2d +{ + private final AccelerationLimitedYoVariable x, y; + + private AccelerationLimitedYoFrameVector2d(AccelerationLimitedYoVariable x, AccelerationLimitedYoVariable y, ReferenceFrame referenceFrame) + { + super(x, y, referenceFrame); + + this.x = x; + this.y = y; + } + + public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + YoDouble maxRate, YoDouble maxAcceleration, double dt, ReferenceFrame referenceFrame) + { + AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); + AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); + + AccelerationLimitedYoFrameVector2d ret = new AccelerationLimitedYoFrameVector2d(x, y, referenceFrame); + + return ret; + } + + + public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + YoDouble maxRate, YoDouble maxAcceleration, double dt, YoFrameVector2d unfilteredVector) + { + AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoX(), dt); + AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoY(), dt); + + AccelerationLimitedYoFrameVector2d ret = new AccelerationLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); + + return ret; + } + + public void setGainsByPolePlacement(double w0, double zeta) + { + x.setGainsByPolePlacement(w0, zeta); + y.setGainsByPolePlacement(w0, zeta); + } + + public void update() + { + x.update(); + y.update(); + } + + public void update(double xUnfiltered, double yUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + } + + public void update(Vector2D vector2dUnfiltered) + { + x.update(vector2dUnfiltered.getX()); + y.update(vector2dUnfiltered.getY()); + } + + public void update(FrameVector2D vector2dUnfiltered) + { + x.update(vector2dUnfiltered.getX()); + y.update(vector2dUnfiltered.getY()); + } + + public void reset() + { + x.reset(); + y.reset(); + } +} + + diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java new file mode 100644 index 00000000..971a4e27 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java @@ -0,0 +1,157 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class AccelerationLimitedYoVariable extends YoDouble +{ + private final double dt; + + private final YoBoolean hasBeenInitialized; + + private final YoDouble smoothedRate; + private final YoDouble smoothedAcceleration; + + private final YoDouble positionGain; + private final YoDouble velocityGain; + + private YoDouble maximumRate; + private YoDouble maximumAcceleration; + + private final YoDouble inputVariable; + + public AccelerationLimitedYoVariable(String name, YoVariableRegistry registry, double maxRate, double maxAcceleration, YoDouble inputVariable, double dt) + { + this(name, registry, null, null, inputVariable, dt); + + maximumRate = new YoDouble(name + "MaxRate", registry); + maximumAcceleration = new YoDouble(name + "MaxAcceleration", registry); + + maximumRate.set(maxRate); + maximumAcceleration.set(maxAcceleration); + } + + public AccelerationLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxRate, YoDouble maxAcceleration, double dt) + { + this(name, registry, maxRate, maxAcceleration, null, dt); + } + + public AccelerationLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxRate, YoDouble maxAcceleration, + YoDouble inputVariable, double dt) + { + super(name, registry); + + if (maxRate != null && maxAcceleration != null) + { + this.maximumRate = maxRate; + this.maximumAcceleration = maxAcceleration; + } + + this.dt = dt; + + hasBeenInitialized = new YoBoolean(name + "HasBeenInitialized", registry); + + smoothedRate = new YoDouble(name + "SmoothedRate", registry); + smoothedAcceleration = new YoDouble(name + "SmoothedAcceleration", registry); + + positionGain = new YoDouble(name + "PositionGain", registry); + velocityGain = new YoDouble(name + "VelocityGain", registry); + + double w0 = 2.0 * Math.PI * 16.0; + double zeta = 1.0; + + setGainsByPolePlacement(w0, zeta); + hasBeenInitialized.set(false); + + this.inputVariable = inputVariable; + } + + public void setMaximumAcceleration(double maximumAcceleration) + { + this.maximumAcceleration.set(maximumAcceleration); + } + + public void setMaximumRate(double maximumRate) + { + this.maximumRate.set(maximumRate); + } + + public void setGainsByPolePlacement(double w0, double zeta) + { + positionGain.set(w0 * w0); + velocityGain.set(2.0 * zeta * w0); + } + + public YoDouble getPositionGain() + { + return positionGain; + } + + public YoDouble getVelocityGain() + { + return velocityGain; + } + + public void update() + { + update(inputVariable.getDoubleValue()); + } + + public void update(double input) + { + if (!hasBeenInitialized.getBooleanValue()) + initialize(input); + + double positionError = input - this.getDoubleValue(); + double acceleration = -velocityGain.getDoubleValue() * smoothedRate.getDoubleValue() + positionGain.getDoubleValue() * positionError; + acceleration = MathTools.clamp(acceleration, -maximumAcceleration.getDoubleValue(), maximumAcceleration.getDoubleValue()); + + smoothedAcceleration.set(acceleration); + smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); + smoothedRate.set(MathTools.clamp(smoothedRate.getDoubleValue(), maximumRate.getDoubleValue())); + this.add(smoothedRate.getDoubleValue() * dt); + } + + public void initialize(double input) + { + this.set(input); + smoothedRate.set(0.0); + smoothedAcceleration.set(0.0); + + this.hasBeenInitialized.set(true); + } + + public void reset() + { + this.hasBeenInitialized.set(false); + smoothedRate.set(0.0); + smoothedAcceleration.set(0.0); + } + + public YoDouble getSmoothedRate() + { + return smoothedRate; + } + + public YoDouble getSmoothedAcceleration() + { + return smoothedAcceleration; + } + + public boolean hasBeenInitialized() + { + return hasBeenInitialized.getBooleanValue(); + } + + public double getMaximumRate() + { + return maximumRate.getDoubleValue(); + } + + public double getMaximumAcceleration() + { + return maximumAcceleration.getDoubleValue(); + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBetaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBetaFilteredYoVariable.java new file mode 100644 index 00000000..fcec3b6a --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBetaFilteredYoVariable.java @@ -0,0 +1,121 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +/** + *

Title:

+ * + *

Description:

+ * + *

Copyright: Copyright (c) 2007

+ * + *

Company:

+ * + * @author not attributable + * @version 1.0 + */ + +/** + * @author thutcheson + *

+ *

+ *

+ *

+ * An AlphaBetaFilteredYoVariable is a filtered version of an input YoVar. + * Either a YoVariable holding the unfiltered val is passed in to the + * constructor and update() is called every tick, or update(double) is + * called every tick. The AlphaBetaFilteredYoVariable updates it's val + * with the current filtered velocity using + * + * xp = x + (dt) v // angular position prediction + * x+ = xp + alpha (xmeas - xp) // adjusted angular position estimate + * v+ = v + beta (xmeas - xp) // adjusted velocity estimate + * + *

+ * 
+ *
+ *         For complete reference see:
+ *                http://www.mstarlabs.com/control/engspeed.html#Ref2
+ *
+ *
+ */ +public class AlphaBetaFilteredYoVariable extends YoDouble +{ + private double alpha = 0.0, beta = 0.0; + + private final double DT; + private YoDouble alphaVariable = null; + private YoDouble betaVariable = null; + + private final YoDouble positionState; + private final YoDouble xMeasuredVariable; + + public AlphaBetaFilteredYoVariable(String name, YoVariableRegistry registry, double alpha, double beta, YoDouble positionVariable, + YoDouble xMeasuredVariable, double DT) + { + super(name, registry); + this.alpha = alpha; + this.beta = beta; + this.DT = DT; + this.positionState = positionVariable; + this.xMeasuredVariable = xMeasuredVariable; + + reset(); + } + + public AlphaBetaFilteredYoVariable(String name, YoVariableRegistry registry, YoDouble alphaVariable, YoDouble betaVariable, + YoDouble positionVariable, YoDouble xMeasuredVariable, double DT) + { + super(name, registry); + this.alphaVariable = alphaVariable; + this.betaVariable = betaVariable; + this.DT = DT; + this.positionState = positionVariable; + this.xMeasuredVariable = xMeasuredVariable; + + reset(); + } + + public void reset() + { + } + + public YoDouble getPositionEstimation() + { + return positionState; + } + + public YoDouble getVelocityEstimation() + { + return this; + } + + public void update() + { + update(positionState.getDoubleValue()); + } + + public void update(double position) + { + if (alphaVariable != null) + { + alpha = alphaVariable.getDoubleValue(); + } + + if (betaVariable != null) + { + beta = betaVariable.getDoubleValue(); + } + + double velocity = this.getDoubleValue(); + + double prediction = position + DT * velocity; // xp = x + (dt) v, position prediction + double error = xMeasuredVariable.getDoubleValue() - prediction; + double newPosition = prediction + alpha * error; // x+ = xp + alpha (xmeas - xp), adjusted position estimate + double newVelocity = velocity + beta * error; // v+ = v + beta (xmeas - xp), adjusted velocity estimate + + positionState.set(newPosition); + this.set(newVelocity); // filter updates its YoVariable value to velocity by convention. + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java new file mode 100644 index 00000000..9ab23e07 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java @@ -0,0 +1,138 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class AlphaFilteredWrappingYoVariable extends AlphaFilteredYoVariable +{ + public static final double EPSILON = 1e-10; + + private double previousUnfilteredVariable; + private final YoDouble unfilteredVariable; + private final YoDouble unfilteredInRangeVariable; + private final YoDouble alphaVariable; + + private final YoDouble temporaryOutputVariable; + private final YoDouble error; + private final double upperLimit; + private final double lowerLimit; + private final double range; + + //wrap the values in [lowerLimit ; upperLimit[ + + public AlphaFilteredWrappingYoVariable(String name, String description, YoVariableRegistry registry, final YoDouble unfilteredVariable, YoDouble alphaVariable, double lowerLimit, double upperLimit) + { + super(name, description, registry, alphaVariable); + this.alphaVariable = alphaVariable; + this.upperLimit = upperLimit; + this.lowerLimit = lowerLimit; + this.range = upperLimit - lowerLimit; + this.unfilteredVariable = unfilteredVariable; + + unfilteredInRangeVariable = new YoDouble(name + "UnfilteredInRangeVariable", registry); + temporaryOutputVariable = new YoDouble(name + "TemporaryOutputVariable", registry); + error = new YoDouble(name + "Error", registry); + + } + + @Override + public void update() + { + update(unfilteredVariable.getDoubleValue()); + } + + + @Override + public void update(double currentPosition) + { + if(!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + previousUnfilteredVariable = unfilteredVariable.getDoubleValue(); + + unfilteredVariableModulo(currentPosition); + + temporaryOutputVariable.set(unfilteredInRangeVariable.getDoubleValue()); + set(unfilteredInRangeVariable.getDoubleValue()); + } + else + { + if (!MathTools.epsilonEquals(currentPosition, previousUnfilteredVariable, EPSILON)) + { + previousUnfilteredVariable = currentPosition; + + unfilteredVariableModulo(currentPosition); + + //calculate the error + double standardError = unfilteredInRangeVariable.getDoubleValue() - getDoubleValue(); + double wrappingError; + if(unfilteredInRangeVariable.getDoubleValue() > getDoubleValue()) + { + wrappingError = lowerLimit - getDoubleValue() + unfilteredInRangeVariable.getDoubleValue() - upperLimit; + } + else + { + wrappingError = upperLimit - getDoubleValue() + unfilteredInRangeVariable.getDoubleValue() - lowerLimit; + } + if(Math.abs(standardError) < Math.abs(wrappingError)) + { + error.set(standardError); + } + else + { + error.set(wrappingError); + } + + //determine if wrapping and move the input if necessary + temporaryOutputVariable.set(getDoubleValue()); + if ((getDoubleValue() + error.getDoubleValue()) >= upperLimit) + { + temporaryOutputVariable.set(getDoubleValue() - range); + } + if ((getDoubleValue() + error.getDoubleValue()) < lowerLimit) + { + temporaryOutputVariable.set(getDoubleValue() + range); + } + } + + temporaryOutputVariable.set(alphaVariable.getDoubleValue() * temporaryOutputVariable.getDoubleValue() + (1.0 - alphaVariable.getDoubleValue()) + * unfilteredInRangeVariable.getDoubleValue()); + + if (temporaryOutputVariable.getDoubleValue() > upperLimit + EPSILON) + { + set(temporaryOutputVariable.getDoubleValue() - range); + } + else if (temporaryOutputVariable.getDoubleValue() <= lowerLimit - EPSILON) + { + set(temporaryOutputVariable.getDoubleValue() + range); + } + else + { + set(temporaryOutputVariable.getDoubleValue()); + } + } + } + + private void unfilteredVariableModulo(double currentPosition) + { + //handle if the input is out of range + boolean rangeNeedsToBeChecked = true; + unfilteredInRangeVariable.set(currentPosition); + + while(rangeNeedsToBeChecked) + { + rangeNeedsToBeChecked = false; + if (unfilteredInRangeVariable.getDoubleValue() >= upperLimit) + { + unfilteredInRangeVariable.set(unfilteredInRangeVariable.getDoubleValue() - range); + rangeNeedsToBeChecked = true; + } + if (unfilteredInRangeVariable.getDoubleValue() < lowerLimit) + { + unfilteredInRangeVariable.set(unfilteredInRangeVariable.getDoubleValue() + range); + rangeNeedsToBeChecked = true; + } + } + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java new file mode 100644 index 00000000..11441ca3 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java @@ -0,0 +1,139 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FramePoint3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.robotics.math.frames.YoFramePoint; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; + +public class AlphaFilteredYoFramePoint extends YoFramePoint +{ + private final AlphaFilteredYoVariable x, y, z; + + private AlphaFilteredYoFramePoint(AlphaFilteredYoVariable x, AlphaFilteredYoVariable y, AlphaFilteredYoVariable z, ReferenceFrame referenceFrame) + { + super(x, y, z, referenceFrame); + + this.x = x; + this.y = y; + this.z = z; + } + + public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + ReferenceFrame referenceFrame) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); + AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha); + + AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, referenceFrame); + + return ret; + } + + public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, + YoDouble alpha, ReferenceFrame referenceFrame) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); + AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha); + + AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, referenceFrame); + + return ret; + } + + + public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + YoFramePoint unfilteredPoint) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoY()); + AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoZ()); + + AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); + + return ret; + } + + + public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, + YoDouble alpha, YoFramePoint unfilteredPoint) + { + // alpha is a YoVariable + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoY()); + AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoZ()); + + AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); + + return ret; + } + + public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, + double alphaX, double alphaY, double alphaZ, YoFramePoint unfilteredPoint) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alphaX, unfilteredPoint.getYoX()); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alphaY, unfilteredPoint.getYoY()); + AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alphaZ, unfilteredPoint.getYoZ()); + + AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); + + return ret; + } + + public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, + YoDouble alphaX, YoDouble alphaY, YoDouble alphaZ, YoFramePoint unfilteredPoint) + { + // alpha is a YoVariable + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alphaX, unfilteredPoint.getYoX()); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alphaY, unfilteredPoint.getYoY()); + AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alphaZ, unfilteredPoint.getYoZ()); + + AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); + + return ret; + } + + public void update() + { + x.update(); + y.update(); + z.update(); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + z.update(zUnfiltered); + } + + public void update(Point3D pointUnfiltered) + { + x.update(pointUnfiltered.getX()); + y.update(pointUnfiltered.getY()); + z.update(pointUnfiltered.getZ()); + } + + public void update(FramePoint3D pointUnfiltered) + { + checkReferenceFrameMatch(pointUnfiltered); + x.update(pointUnfiltered.getX()); + y.update(pointUnfiltered.getY()); + z.update(pointUnfiltered.getZ()); + } + + public void reset() + { + x.reset(); + y.reset(); + z.reset(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java new file mode 100644 index 00000000..1c58601c --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java @@ -0,0 +1,102 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FramePoint2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple2D.Point2D; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.robotics.math.frames.YoFramePoint2d; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; + +public class AlphaFilteredYoFramePoint2d extends YoFramePoint2d +{ + private final AlphaFilteredYoVariable x, y; + + private AlphaFilteredYoFramePoint2d(AlphaFilteredYoVariable x, AlphaFilteredYoVariable y, ReferenceFrame referenceFrame) + { + super(x, y, referenceFrame); + + this.x = x; + this.y = y; + } + + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); + + AlphaFilteredYoFramePoint2d ret = new AlphaFilteredYoFramePoint2d(x, y, referenceFrame); + + return ret; + } + + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble alpha, ReferenceFrame referenceFrame) + { + return createAlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); + } + + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, YoDouble alpha, ReferenceFrame referenceFrame) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, alpha); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, registry, alpha); + + AlphaFilteredYoFramePoint2d ret = new AlphaFilteredYoFramePoint2d(x, y, referenceFrame); + + return ret; + } + + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, YoFramePoint2d unfilteredPoint) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoY()); + + AlphaFilteredYoFramePoint2d ret = new AlphaFilteredYoFramePoint2d(x, y, unfilteredPoint.getReferenceFrame()); + + return ret; + } + + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble alpha, YoFramePoint2d unfilteredPoint) + { + // alpha is a YoVariable + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoY()); + + AlphaFilteredYoFramePoint2d ret = new AlphaFilteredYoFramePoint2d(x, y, unfilteredPoint.getReferenceFrame()); + + return ret; + } + + public void update() + { + x.update(); + y.update(); + } + + public void update(double xUnfiltered, double yUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + } + + public void update(Point2D point2dUnfiltered) + { + x.update(point2dUnfiltered.getX()); + y.update(point2dUnfiltered.getY()); + } + + public void update(FramePoint2D point2dUnfiltered) + { + checkReferenceFrameMatch(point2dUnfiltered); + x.update(point2dUnfiltered.getX()); + y.update(point2dUnfiltered.getY()); + } + + public void reset() + { + x.reset(); + y.reset(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java new file mode 100644 index 00000000..5568a4aa --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java @@ -0,0 +1,95 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.robotics.math.frames.YoFrameQuaternion; + +public class AlphaFilteredYoFrameQuaternion extends YoFrameQuaternion implements ProcessingYoVariable +{ + private final YoFrameQuaternion unfilteredQuaternion; + private final YoDouble alpha; + private final YoBoolean hasBeenCalled; + private final Quaternion qMeasured = new Quaternion(); + private final Quaternion qPreviousFiltered = new Quaternion(); + private final Quaternion qNewFiltered = new Quaternion(); + + public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, double alpha, + YoVariableRegistry registry) + { + this(namePrefix, nameSuffix, unfilteredQuaternion, new YoDouble(namePrefix + "Alpha", registry), registry); + this.setAlpha(alpha); + } + + public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoDouble alpha, ReferenceFrame referenceFrame, + YoVariableRegistry registry) + { + this(namePrefix, nameSuffix, null, alpha, referenceFrame, registry); + } + + public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, YoDouble alpha, + YoVariableRegistry registry) + { + this(namePrefix, nameSuffix, unfilteredQuaternion, alpha, unfilteredQuaternion.getReferenceFrame(), registry); + } + + private AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, YoDouble alpha, + ReferenceFrame referenceFrame, YoVariableRegistry registry) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + this.unfilteredQuaternion = unfilteredQuaternion; + this.alpha = alpha; + this.hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); + } + + @Override + public void update() + { + if (unfilteredQuaternion == null) + { + throw new NullPointerException("AlphaFilteredYoFrameQuaternion must be constructed with a non null " + + "quaternion variable to call update(), otherwise use update(Quat4d)"); + } + + qMeasured.set(unfilteredQuaternion); + update(qMeasured); + } + + public void update(Quaternion qMeasured) + { + if (hasBeenCalled.getBooleanValue()) + { + qPreviousFiltered.set(this); + + qNewFiltered.interpolate(qMeasured, qPreviousFiltered, alpha.getDoubleValue()); // qPreviousFiltered 'gets multiplied by alpha' + set(qNewFiltered); + } + else + { + set(qMeasured); + hasBeenCalled.set(true); + } + } + + public void setAlpha(double alpha) + { + this.alpha.set(alpha); + } + + public void setBreakFrequency(double breakFrequencyHertz, double dt) + { + this.alpha.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequencyHertz, dt)); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public YoFrameQuaternion getUnfilteredQuaternion() + { + return unfilteredQuaternion; + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java new file mode 100644 index 00000000..32bca05c --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java @@ -0,0 +1,127 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.robotics.math.frames.YoFramePoint; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.robotics.math.frames.YoFrameVector; + +public class AlphaFilteredYoFrameVector extends YoFrameVector implements ProcessingYoVariable +{ + private final AlphaFilteredYoVariable x, y, z; + + private AlphaFilteredYoFrameVector(AlphaFilteredYoVariable x, AlphaFilteredYoVariable y, AlphaFilteredYoVariable z, ReferenceFrame referenceFrame) + { + super(x, y, z, referenceFrame); + + this.x = x; + this.y = y; + this.z = z; + } + + public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + ReferenceFrame referenceFrame) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); + AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha); + + AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, referenceFrame); + + return ret; + } + + public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + YoDouble alpha, ReferenceFrame referenceFrame) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); + AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha); + + AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, referenceFrame); + + return ret; + } + + + public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + YoFrameVector unfilteredVector) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoY()); + AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoZ()); + + AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); + + return ret; + } + + + public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + YoDouble alpha, YoFrameVector unfilteredVector) + { + // alpha is a YoVariable + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoY()); + AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoZ()); + + AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); + + return ret; + } + + public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + YoDouble alpha, YoFramePoint unfilteredPosition) + { + // alpha is a YoVariable + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPosition.getYoX()); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPosition.getYoY()); + AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredPosition.getYoZ()); + + AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, unfilteredPosition.getReferenceFrame()); + + return ret; + } + + public void update() + { + x.update(); + y.update(); + z.update(); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + z.update(zUnfiltered); + } + + public void update(Vector3D vectorUnfiltered) + { + x.update(vectorUnfiltered.getX()); + y.update(vectorUnfiltered.getY()); + z.update(vectorUnfiltered.getZ()); + } + + public void update(FrameVector3D vectorUnfiltered) + { + checkReferenceFrameMatch(vectorUnfiltered); + x.update(vectorUnfiltered.getX()); + y.update(vectorUnfiltered.getY()); + z.update(vectorUnfiltered.getZ()); + } + + public void reset() + { + x.reset(); + y.reset(); + z.reset(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java new file mode 100644 index 00000000..ac4a535f --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java @@ -0,0 +1,107 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.robotics.math.frames.YoFrameVector2d; + +public class AlphaFilteredYoFrameVector2d extends YoFrameVector2d +{ + private final AlphaFilteredYoVariable x, y; + + private AlphaFilteredYoFrameVector2d(AlphaFilteredYoVariable x, AlphaFilteredYoVariable y, ReferenceFrame referenceFrame) + { + super(x, y, referenceFrame); + + this.x = x; + this.y = y; + } + + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); + + AlphaFilteredYoFrameVector2d ret = new AlphaFilteredYoFrameVector2d(x, y, referenceFrame); + + return ret; + } + + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble alpha, ReferenceFrame referenceFrame) + { + return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); + } + + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, YoDouble alpha, ReferenceFrame referenceFrame) + { + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, alpha); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, registry, alpha); + + AlphaFilteredYoFrameVector2d ret = new AlphaFilteredYoFrameVector2d(x, y, referenceFrame); + + return ret; + } + + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, YoFrameVector2d unfilteredVector) + { + // alpha is a double + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoY()); + + AlphaFilteredYoFrameVector2d ret = new AlphaFilteredYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); + + return ret; + } + + + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble alpha, YoFrameVector2d unfilteredVector) + { + return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, unfilteredVector); + } + + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, YoDouble alpha, YoFrameVector2d unfilteredVector) + { + // alpha is a YoVariable + AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, alpha, unfilteredVector.getYoX()); + AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, registry, alpha, unfilteredVector.getYoY()); + + AlphaFilteredYoFrameVector2d ret = new AlphaFilteredYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); + + return ret; + } + + public void update() + { + x.update(); + y.update(); + } + + public void update(double xUnfiltered, double yUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + } + + public void update(Vector2D vector2dUnfiltered) + { + x.update(vector2dUnfiltered.getX()); + y.update(vector2dUnfiltered.getY()); + } + + public void update(FrameVector2D vector2dUnfiltered) + { + checkReferenceFrameMatch(vector2dUnfiltered); + x.update(vector2dUnfiltered.getX()); + y.update(vector2dUnfiltered.getY()); + } + + public void reset() + { + x.reset(); + y.reset(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java new file mode 100644 index 00000000..ab994d3c --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java @@ -0,0 +1,171 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +/** + * @author jrebula + *

+ *

+ * LittleDogVersion06: + * us.ihmc.LearningLocomotion.Version06.util.YoAlphaFilteredVariable, + * 9:34:00 AM, Aug 29, 2006 + *

= + *

+ * A YoAlphaFilteredVariable is a filtered version of an input YoVar. + * Either a YoVariable holding the unfiltered val is passed in to the + * constructor and update() is called every tick, or update(double) is + * called every tick. The YoAlphaFilteredVariable updates it's val + * with the current filtered version using + *

+ * 
+ *            filtered_{n} = alpha * filtered_{n-1} + (1 - alpha) * raw_{n}
+ *
+ * + * For alpha=0 -> no filtered + * For alpha=1 -> 100% filtered, no use of raw signal + */ +public class AlphaFilteredYoVariable extends YoDouble implements ProcessingYoVariable +{ + private final YoDouble alphaVariable; + + private final YoDouble position; + protected final YoBoolean hasBeenCalled; + + public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, double alpha) + { + this(name, registry, alpha, null); + } + + public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, double alpha, YoDouble positionVariable) + { + super(name,registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + this.alphaVariable = new YoDouble(name + "AlphaVariable", registry); + this.alphaVariable.set(alpha); + this.position = positionVariable; + reset(); + } + + public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, YoDouble alphaVariable) + { + this(name, "", registry, alphaVariable, null); + } + + public AlphaFilteredYoVariable(String name, String description, YoVariableRegistry registry, YoDouble alphaVariable) + { + this(name, description, registry, alphaVariable, null); + } + + + public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, YoDouble alphaVariable, YoDouble positionVariable) + { + this(name, "", registry, alphaVariable, positionVariable); + } + + public AlphaFilteredYoVariable(String name, String description, YoVariableRegistry registry, YoDouble alphaVariable, YoDouble positionVariable) + { + super(name, description, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", description, registry); + this.position = positionVariable; + this.alphaVariable = alphaVariable; + + reset(); + } + + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (position == null) + { + throw new NullPointerException("YoAlphaFilteredVariable must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position.getDoubleValue()); + } + + public void update(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + set(currentPosition); + } + + + set(alphaVariable.getDoubleValue() * getDoubleValue() + (1.0 - alphaVariable.getDoubleValue()) * currentPosition); + + } + + public void setAlpha(double alpha) + { + this.alphaVariable.set(alpha); + } + + /** + * This method is replaced by computeAlphaGivenBreakFrequencyProperly. It is fine to keep using this method is currently using it, knowing that + * the actual break frequency is not exactly what you are asking for. + * + * @param breakFrequencyInHertz + * @param dt + * @return + */ + @Deprecated + public static double computeAlphaGivenBreakFrequency(double breakFrequencyInHertz, double dt) + { + if (Double.isInfinite(breakFrequencyInHertz)) + return 0.0; + + double alpha = 1.0 - breakFrequencyInHertz * 2.0 * Math.PI * dt; + + alpha = MathTools.clamp(alpha, 0.0, 1.0); + + return alpha; + } + + public static double computeAlphaGivenBreakFrequencyProperly(double breakFrequencyInHertz, double dt) + { + if (Double.isInfinite(breakFrequencyInHertz)) + return 0.0; + + double omega = 2.0 * Math.PI * breakFrequencyInHertz; + double alpha = (1.0 - omega * dt / 2.0) / (1.0 + omega * dt / 2.0); + alpha = MathTools.clamp(alpha, 0.0, 1.0); + return alpha; + } + + public static double computeBreakFrequencyGivenAlpha(double alpha, double dt) + { + return (1.0 - alpha) / (Math.PI * dt + Math.PI * alpha * dt); + } + + 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 boolean getHasBeenCalled() + { + return hasBeenCalled.getBooleanValue(); + } + +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java new file mode 100644 index 00000000..01c22b1e --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java @@ -0,0 +1,77 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.robotics.math.frames.YoFramePoint; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.robotics.math.frames.YoFrameVector; + +public class BacklashCompensatingVelocityYoFrameVector extends YoFrameVector +{ + private final BacklashCompensatingVelocityYoVariable xDot, yDot, zDot; + + public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensatingVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, YoDouble slopTime, + YoVariableRegistry registry, YoFrameVector yoFrameVectorToDifferentiate) + { + BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoX(), dt, slopTime, + registry); + BacklashCompensatingVelocityYoVariable yDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoY(), dt, slopTime, + registry); + BacklashCompensatingVelocityYoVariable zDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoZ(), dt, slopTime, + registry); + + BacklashCompensatingVelocityYoFrameVector ret = new BacklashCompensatingVelocityYoFrameVector(xDot, yDot, zDot, registry, yoFrameVectorToDifferentiate); + + return ret; + } + + public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensatingVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, YoDouble slopTime, + YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) + { + BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoX(), dt, slopTime, + registry); + BacklashCompensatingVelocityYoVariable yDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoY(), dt, slopTime, + registry); + BacklashCompensatingVelocityYoVariable zDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoZ(), dt, slopTime, + registry); + + BacklashCompensatingVelocityYoFrameVector ret = new BacklashCompensatingVelocityYoFrameVector(xDot, yDot, zDot, registry, yoFramePointToDifferentiate); + + return ret; + } + + private BacklashCompensatingVelocityYoFrameVector(BacklashCompensatingVelocityYoVariable xDot, BacklashCompensatingVelocityYoVariable yDot, BacklashCompensatingVelocityYoVariable zDot, + YoVariableRegistry registry, YoFrameVector yoFrameVectorToDifferentiate) + { + super(xDot, yDot, zDot, yoFrameVectorToDifferentiate.getReferenceFrame()); + + this.xDot = xDot; + this.yDot = yDot; + this.zDot = zDot; + } + + private BacklashCompensatingVelocityYoFrameVector(BacklashCompensatingVelocityYoVariable xDot, BacklashCompensatingVelocityYoVariable yDot, BacklashCompensatingVelocityYoVariable zDot, + YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) + { + super(xDot, yDot, zDot, yoFramePointToDifferentiate.getReferenceFrame()); + + this.xDot = xDot; + this.yDot = yDot; + this.zDot = zDot; + } + + public void update() + { + xDot.update(); + yDot.update(); + zDot.update(); + } + + public void reset() + { + xDot.reset(); + yDot.reset(); + zDot.reset(); + } + +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java new file mode 100644 index 00000000..5203f46d --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java @@ -0,0 +1,240 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoEnum; + +public class BacklashCompensatingVelocityYoVariable extends YoDouble implements ProcessingYoVariable +{ + private final double dt; + + private final YoDouble alphaVariable; + private final YoDouble position; + + private final YoDouble lastPosition; + private final YoBoolean hasBeenCalled; + + private final YoEnum backlashState; + private final YoDouble slopTime; + + private final YoDouble timeInState; + + public BacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, YoDouble positionVariable, double dt, + YoDouble slopTime, YoVariableRegistry registry) + { + super(name, description, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + + backlashState = new YoEnum(name + "BacklashState", registry, BacklashState.class, true); + backlashState.set(null); + timeInState = new YoDouble(name + "TimeInState", registry); + + position = positionVariable; + + this.alphaVariable = alphaVariable; + this.slopTime = slopTime; + + this.dt = dt; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + + reset(); + } + + public BacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, double dt, YoDouble slopTime, + YoVariableRegistry registry) + { + super(name, description, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + + backlashState = new YoEnum(name + "BacklashState", registry, BacklashState.class, true); + backlashState.set(null); + timeInState = new YoDouble(name + "timeInState", registry); + + this.position = null; + + this.alphaVariable = alphaVariable; + this.slopTime = slopTime; + + this.dt = dt; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + backlashState.set(null); + } + + public void update() + { + if (position == null) + { + throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position.getDoubleValue()); + } + + // public void updateForAngles() + // { + // if (position == null) + // { + // throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + // + "position variable to call update(), otherwise use update(double)"); + // } + // + // updateForAngles(position.getDoubleValue()); + // } + + public void update(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + lastPosition.set(currentPosition); + set(0.0); + } + + timeInState.add(dt); + + boolean sloppy = false; + + double difference = currentPosition - lastPosition.getDoubleValue(); + + if (backlashState.getEnumValue() == null) + { + if (difference > 0.0) + backlashState.set(BacklashState.FORWARD_OK); + else if (difference < 0.0) + backlashState.set(BacklashState.BACKWARD_OK); + } + else + { + switch (backlashState.getEnumValue()) + { + case BACKWARD_OK: + { + if (difference > 0.0) + { + backlashState.set(BacklashState.FORWARD_SLOP); + sloppy = true; + timeInState.set(0.0); + } + break; + } + case FORWARD_OK: + { + if (difference < 0.0) + { + backlashState.set(BacklashState.BACKWARD_SLOP); + sloppy = true; + timeInState.set(0.0); + } + + break; + } + case BACKWARD_SLOP: + { + sloppy = true; + + if (difference > 0.0) + { + backlashState.set(BacklashState.FORWARD_SLOP); + sloppy = true; + timeInState.set(0.0); + } + else if (timeInState.getDoubleValue() > slopTime.getDoubleValue()) + { + backlashState.set(BacklashState.BACKWARD_OK); + sloppy = false; + timeInState.set(0.0); + } + + break; + } + case FORWARD_SLOP: + { + sloppy = true; + + if (difference < 0.0) + { + backlashState.set(BacklashState.BACKWARD_SLOP); + sloppy = true; + timeInState.set(0.0); + } + else if (timeInState.getDoubleValue() > slopTime.getDoubleValue()) + { + backlashState.set(BacklashState.FORWARD_OK); + sloppy = false; + timeInState.set(0.0); + } + break; + } + } + } + + if (sloppy) + { + double percent = timeInState.getDoubleValue() / slopTime.getDoubleValue(); + percent = MathTools.clamp(percent, 0.0, 1.0); + if (Double.isNaN(percent)) + percent = 1.0; + + double scaleFactor = percent; + + difference = scaleFactor * difference; + } + + updateUsingDifference(difference); + + lastPosition.set(currentPosition); + + } + + // public void updateForAngles(double currentPosition) + // { + // if (!hasBeenCalled.getBooleanValue()) + // { + // hasBeenCalled.set(true); + // lastPosition.set(currentPosition); + // set(0.0); + // } + // + // double difference = AngleTools.computeAngleDifferenceMinusPiToPi(currentPosition, lastPosition.getDoubleValue()); + // + // updateUsingDifference(difference); + // + // lastPosition.set(currentPosition); + // } + + private void updateUsingDifference(double difference) + { + double previousFilteredDerivative = getDoubleValue(); + double currentRawDerivative = difference / dt; + + double alpha = alphaVariable.getDoubleValue(); + set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); + } + + public void setAlpha(double alpha) + { + this.alphaVariable.set(alpha); + } + + public void setSlopTime(double slopTime) + { + this.slopTime.set(slopTime); + } + + private enum BacklashState + { + BACKWARD_OK, FORWARD_OK, BACKWARD_SLOP, FORWARD_SLOP; + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java new file mode 100644 index 00000000..6875c01d --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java @@ -0,0 +1,155 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoEnum; + +/** + * This does essentially the same as RevisedBacklashCompensatingVelocityYoVariable, except it takes a velocity signal as input. + * + */ +public class BacklashProcessingYoVariable extends YoDouble implements ProcessingYoVariable +{ + private final YoDouble velocity; + + private final YoBoolean hasBeenCalled; + + private final YoEnum backlashState; + private final YoDouble slopTime; + + private final YoDouble timeSinceSloppy; + + private final double dt; + + public BacklashProcessingYoVariable(String name, String description, double dt, YoDouble slopTime, YoVariableRegistry registry) + { + this(name, description, null, dt, slopTime, registry); + } + + public BacklashProcessingYoVariable(String name, String description, YoDouble velocityVariable, double dt, YoDouble slopTime, + YoVariableRegistry registry) + { + super(name, description, registry); + + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + + backlashState = new YoEnum(name + "BacklashState", registry, BacklashState.class, true); + backlashState.set(null); + timeSinceSloppy = new YoDouble(name + "TimeSinceSloppy", registry); + + velocity = velocityVariable; + + this.slopTime = slopTime; + + this.dt = dt; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + backlashState.set(null); + } + + public void update() + { + if (velocity == null) + { + throw new NullPointerException( + "BacklashProcessingYoVariable must be constructed with a non null " + "velocity variable to call update(), otherwise use update(double)"); + } + + update(velocity.getDoubleValue()); + } + + public void update(double currentVelocity) + { + if (backlashState.getEnumValue() == null) + { + backlashState.set(BacklashState.FORWARD_OK); + } + + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + set(currentVelocity); + } + + timeSinceSloppy.add(dt); + + switch (backlashState.getEnumValue()) + { + case BACKWARD_OK: + { + if (currentVelocity > 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.FORWARD_SLOP); + } + + break; + } + + case FORWARD_OK: + { + if (currentVelocity < 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.BACKWARD_SLOP); + } + + break; + } + + case BACKWARD_SLOP: + { + if (currentVelocity > 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.FORWARD_SLOP); + } + else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) + { + backlashState.set(BacklashState.BACKWARD_OK); + } + + break; + } + + case FORWARD_SLOP: + { + if (currentVelocity < 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.BACKWARD_SLOP); + } + else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) + { + backlashState.set(BacklashState.FORWARD_OK); + } + + break; + } + } + + double percent = timeSinceSloppy.getDoubleValue() / slopTime.getDoubleValue(); + percent = MathTools.clamp(percent, 0.0, 1.0); + if (Double.isNaN(percent) || slopTime.getDoubleValue() < dt) + percent = 1.0; + + this.set(percent * currentVelocity); + } + + public void setSlopTime(double slopTime) + { + this.slopTime.set(slopTime); + } + + private enum BacklashState + { + BACKWARD_OK, FORWARD_OK, BACKWARD_SLOP, FORWARD_SLOP; + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java new file mode 100644 index 00000000..807eb68d --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java @@ -0,0 +1,85 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FramePoint2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple2D.Point2D; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.robotics.math.frames.YoFramePoint2d; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; + +public class BetaFilteredYoFramePoint2d extends YoFramePoint2d +{ + private final BetaFilteredYoVariable x, y; + + private BetaFilteredYoFramePoint2d(BetaFilteredYoVariable x, BetaFilteredYoVariable y, ReferenceFrame referenceFrame) + { + super(x, y, referenceFrame); + + this.x = x; + this.y = y; + } + + public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, ReferenceFrame referenceFrame) + { + // beta is a int + BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta); + BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta); + + BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, referenceFrame); + + return ret; + } + + public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, int beta, ReferenceFrame referenceFrame) + { + // beta is a int + BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, beta); + BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, registry, beta); + + BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, referenceFrame); + + return ret; + } + + public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFramePoint2d unfilteredPoint) + { + // beta is a int + BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoX()); + BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoY()); + + BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, unfilteredPoint.getReferenceFrame()); + + return ret; + } + + public void update() + { + x.update(); + y.update(); + } + + public void update(double xUnfiltered, double yUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + } + + public void update(Point2D point2dUnfiltered) + { + x.update(point2dUnfiltered.getX()); + y.update(point2dUnfiltered.getY()); + } + + public void update(FramePoint2D point2dUnfiltered) + { + checkReferenceFrameMatch(point2dUnfiltered); + x.update(point2dUnfiltered.getX()); + y.update(point2dUnfiltered.getY()); + } + + public void reset() + { + x.reset(); + y.reset(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java new file mode 100644 index 00000000..607bbc78 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java @@ -0,0 +1,75 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.robotics.math.frames.YoFrameVector2d; + +public class BetaFilteredYoFrameVector2d extends YoFrameVector2d +{ + private final BetaFilteredYoVariable x, y; + + private BetaFilteredYoFrameVector2d(BetaFilteredYoVariable x, BetaFilteredYoVariable y, ReferenceFrame referenceFrame) + { + super(x, y, referenceFrame); + + this.x = x; + this.y = y; + } + + public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, ReferenceFrame referenceFrame) + { + // beta is a int + BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta); + BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta); + + BetaFilteredYoFrameVector2d ret = new BetaFilteredYoFrameVector2d(x, y, referenceFrame); + + return ret; + } + + public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFrameVector2d unfilteredVector) + { + // beta is a int + BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoX()); + BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoY()); + + BetaFilteredYoFrameVector2d ret = new BetaFilteredYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); + + return ret; + } + + + public void update() + { + x.update(); + y.update(); + } + + public void update(double xUnfiltered, double yUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + } + + public void update(Vector2D vector2dUnfiltered) + { + x.update(vector2dUnfiltered.getX()); + y.update(vector2dUnfiltered.getY()); + } + + public void update(FrameVector2D vector2dUnfiltered) + { + checkReferenceFrameMatch(vector2dUnfiltered); + x.update(vector2dUnfiltered.getX()); + y.update(vector2dUnfiltered.getY()); + } + + public void reset() + { + x.reset(); + y.reset(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java new file mode 100644 index 00000000..464695c6 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java @@ -0,0 +1,105 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +/** + * @author thutcheson + *

+ *

+ *

+ *

+ * A BetaFilteredYoVariable is a filtered version of an input YoVar. + * Either a YoVariable holding the unfiltered val is passed in to the + * constructor and update() is called every tick, or update(double) is + * called every tick. The BetaFilteredYoVariable updates it's val + * with the current filtered version using + *

+ * 
+ *            filtered_{n} = (raw_{0} + ... + raw_{n-1} + raw_{n}) / n
+ *
+ */ +public class BetaFilteredYoVariable extends YoDouble +{ + private int beta; + private int index = 0; + @SuppressWarnings("unused") + private final YoDouble betaVariable; + + private final YoDouble position; + + private static double raw[]; + + private final YoBoolean hasBeenCalled; + + public BetaFilteredYoVariable(String name, YoVariableRegistry registry, int beta) + { + this(name, "", registry, beta, null); + } + + public BetaFilteredYoVariable(String name, String description, YoVariableRegistry registry, int beta) + { + this(name, description, registry, beta, null); + } + + public BetaFilteredYoVariable(String name, YoVariableRegistry registry, int beta, YoDouble positionVariable) + { + this(name, "", registry, beta, positionVariable); + } + + public BetaFilteredYoVariable(String name, String description, YoVariableRegistry registry, int beta, YoDouble positionVariable) + { + super(name, description, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + + this.beta = beta; + this.position = positionVariable; + this.betaVariable = null; + + raw = new double[beta]; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + + for (int i = 0; i < beta; i++) + { + set(0.0); + } + } + + public void update() + { + if (position == null) + { + throw new NullPointerException("BetaFilteredYoVariable must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position.getDoubleValue()); + } + public void update(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + set(currentPosition); + } + + raw[index++] = currentPosition; + if (index == beta) + index = 0; + set(0.0); + + for (int i = 0; i < beta; i++) + { + set(getDoubleValue() + raw[i]); + } + + set(getDoubleValue() / beta); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java new file mode 100644 index 00000000..7667ff72 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java @@ -0,0 +1,72 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.robotics.math.frames.YoFrameTuple; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.robotics.math.frames.YoFrameVector; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class DeadzoneYoFrameVector extends YoFrameVector implements ProcessingYoVariable +{ + private final DeadzoneYoVariable x, y, z; + + private DeadzoneYoFrameVector(DeadzoneYoVariable x, DeadzoneYoVariable y, DeadzoneYoVariable z, ReferenceFrame referenceFrame) + { + super(x, y, z, referenceFrame); + + this.x = x; + this.y = y; + this.z = z; + } + + public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, YoVariableRegistry registry, YoDouble deadzoneSize, ReferenceFrame referenceFrame) + { + return createDeadzoneYoFrameVector(namePrefix, "", registry, deadzoneSize, referenceFrame); + } + + public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble deadzoneSize, ReferenceFrame referenceFrame) + { + DeadzoneYoVariable x = new DeadzoneYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), deadzoneSize, registry); + DeadzoneYoVariable y = new DeadzoneYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), deadzoneSize, registry); + DeadzoneYoVariable z = new DeadzoneYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), deadzoneSize, registry); + + DeadzoneYoFrameVector ret = new DeadzoneYoFrameVector(x, y, z, referenceFrame); + + return ret; + } + + public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, YoVariableRegistry registry, YoDouble deadzoneSize, YoFrameTuple rawTuple) + { + return createDeadzoneYoFrameVector(namePrefix, "", registry, deadzoneSize, rawTuple); + } + + public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble deadzoneSize, YoFrameTuple rawTuple) + { + DeadzoneYoVariable x = new DeadzoneYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), rawTuple.getYoX(), deadzoneSize, registry); + DeadzoneYoVariable y = new DeadzoneYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), rawTuple.getYoY(), deadzoneSize, registry); + DeadzoneYoVariable z = new DeadzoneYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), rawTuple.getYoZ(), deadzoneSize, registry); + + DeadzoneYoFrameVector ret = new DeadzoneYoFrameVector(x, y, z, rawTuple.getReferenceFrame()); + + return ret; + } + + @Override + public void update() + { + x.update(); + y.update(); + z.update(); + } + + public void update(FrameTuple3DReadOnly frameTuple) + { + checkReferenceFrameMatch(frameTuple); + + x.update(frameTuple.getX()); + y.update(frameTuple.getY()); + z.update(frameTuple.getZ()); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java new file mode 100644 index 00000000..f49b922f --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java @@ -0,0 +1,69 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class DeadzoneYoVariable extends YoDouble implements ProcessingYoVariable +{ + private final YoDouble deadzoneSize; + private final YoDouble inputVariable; + + public DeadzoneYoVariable(String name, YoDouble deadzoneSize, YoVariableRegistry registry) + { + super(name, registry); + this.inputVariable = null; + this.deadzoneSize = deadzoneSize; + } + + public DeadzoneYoVariable(String name, YoDouble inputVariable, YoDouble deadzoneSize, YoVariableRegistry registry) + { + super(name, registry); + this.inputVariable = inputVariable; + this.deadzoneSize = deadzoneSize; + } + + public void update() + { + if (inputVariable == null) + throw new NullPointerException("DeadzoneYoVariable must be constructed with a non null " + + "input variable to call update(), otherwise use update(double)"); + update(inputVariable.getDoubleValue()); + } + + public void update(double valueToBeCorrected) + { + if (valueToBeCorrected >= deadzoneSize.getDoubleValue()) + { + super.set(valueToBeCorrected - deadzoneSize.getDoubleValue()); + } + else if (valueToBeCorrected <= -deadzoneSize.getDoubleValue()) + { + super.set(valueToBeCorrected + deadzoneSize.getDoubleValue()); + } + else + { + super.set(0.0); + } + } + + public static void main(String[] args) + { + YoVariableRegistry registry = new YoVariableRegistry("test"); + YoDouble deadzoneSize = new YoDouble("deadzoneSize", registry); + YoDouble input = new YoDouble("input", registry); + deadzoneSize.set(2.0); + DeadzoneYoVariable testDeadzone = new DeadzoneYoVariable("testDeadZone", input,deadzoneSize,registry); + + for(int i = -50; i < 51; i++) + { + input.set((double)i); + testDeadzone.update(); + System.out.println("//////////////////////////"); + System.out.println("uncorrected = " + (double)i); + System.out.println("corrected = " + testDeadzone.getDoubleValue()); + } + + System.out.println("done"); + } + +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java new file mode 100644 index 00000000..e010bacf --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java @@ -0,0 +1,68 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class DelayedYoBoolean extends YoBoolean +{ + private final YoBoolean variableToDelay; + + private final YoBoolean[] previousYoVariables; + + public DelayedYoBoolean(String name, String description, YoBoolean variableToDelay, int ticksToDelay, YoVariableRegistry registry) + { + super(name, description, registry); + + this.variableToDelay = variableToDelay; + previousYoVariables = new YoBoolean[ticksToDelay]; + + for (int i = 0; i < ticksToDelay; i++) + { + previousYoVariables[i] = new YoBoolean(name + "_previous" + i, registry); + previousYoVariables[i].set(variableToDelay.getBooleanValue()); + } + + this.set(variableToDelay.getBooleanValue()); + } + + public void update() + { + if (previousYoVariables.length == 0) + { + this.set(variableToDelay.getBooleanValue()); + return; + } + + this.set(previousYoVariables[0].getBooleanValue()); + + for (int i = 0; i < previousYoVariables.length - 1; i++) + { + previousYoVariables[i].set(previousYoVariables[i + 1].getBooleanValue()); + } + + previousYoVariables[previousYoVariables.length - 1].set(variableToDelay.getBooleanValue()); + } + + public void reset() + { + for (YoBoolean var : previousYoVariables) + { + var.set(variableToDelay.getBooleanValue()); + } + this.set(variableToDelay.getBooleanValue()); + } + + void getInternalState(String inputString, Boolean ifDebug) + { + if (!ifDebug) + return; + + String string = inputString + "\nvalue = " + this.getBooleanValue() + "\n"; + for (int i = 0; i < previousYoVariables.length; i++) + { + string = string + i + " = " + previousYoVariables[i].getBooleanValue() + "\n"; + } + string = string + "variableToDelay = " + variableToDelay.getBooleanValue() + "\n"; + System.out.println(string); + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java new file mode 100644 index 00000000..b87555d6 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java @@ -0,0 +1,54 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class DelayedYoDouble extends YoDouble +{ + private final YoDouble variableToDelay; + + private final YoDouble[] previousYoVariables; + + public DelayedYoDouble(String name, String description, YoDouble variableToDelay, int ticksToDelay, YoVariableRegistry registry) + { + super(name, description, registry); + + this.variableToDelay = variableToDelay; + previousYoVariables = new YoDouble[ticksToDelay]; + + for (int i = 0; i < ticksToDelay; i++) + { + previousYoVariables[i] = new YoDouble(name + "_previous" + i, registry); + previousYoVariables[i].set(variableToDelay.getDoubleValue()); + } + + this.set(variableToDelay.getDoubleValue()); + } + + public void update() + { + if (previousYoVariables.length == 0) + { + this.set(variableToDelay.getDoubleValue()); + return; + } + + this.set(previousYoVariables[0].getDoubleValue()); + + for (int i = 0; i < previousYoVariables.length - 1; i++) + { + previousYoVariables[i].set(previousYoVariables[i + 1].getDoubleValue()); + } + + previousYoVariables[previousYoVariables.length - 1].set(variableToDelay.getDoubleValue()); + } + + public void reset() + { + for (YoDouble var : previousYoVariables) + { + var.set(variableToDelay.getDoubleValue()); + } + this.set(variableToDelay.getDoubleValue()); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java new file mode 100644 index 00000000..5ab166ef --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java @@ -0,0 +1,69 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +/** + * + * @author Doug Stephen (dstephen@ihmc.us) + */ +public class DeltaLimitedYoVariable extends YoDouble +{ + private final YoDouble maxDelta; + private final YoDouble actual; + private final YoDouble desired; + private final YoBoolean isLimitingActive; + + public DeltaLimitedYoVariable(String name, YoVariableRegistry registry, double maxDelta) + { + super(name, registry); + + this.maxDelta = new YoDouble(name + "MaxAllowedDelta", registry); + this.maxDelta.set(Math.abs(maxDelta)); + this.actual = new YoDouble(name + "Actual", registry); + this.desired = new YoDouble(name + "Desired", registry); + this.isLimitingActive = new YoBoolean(name + "IsLimitingActive", registry); + isLimitingActive.set(false); + } + + public void setMaxDelta(double maxDelta) + { + this.maxDelta.set(Math.abs(maxDelta)); + } + + public void updateOutput(double actual, double desired) + { + this.desired.set(desired); + this.actual.set(actual); + updateOutput(); + } + + public boolean isLimitingActive() + { + return isLimitingActive.getBooleanValue(); + } + + private void updateOutput() + { + double actualDoubleValue = actual.getDoubleValue(); + double desiredDoubleValue = desired.getDoubleValue(); + double maxDeltaDoubleValue = Math.abs(maxDelta.getDoubleValue()); + double rawDelta = actualDoubleValue - desiredDoubleValue; + double sign = Math.signum(rawDelta); + double requestedDelta = Math.abs(rawDelta); + double overshoot = maxDeltaDoubleValue - requestedDelta; + + if(overshoot < 0) + { + desiredDoubleValue = actualDoubleValue - maxDeltaDoubleValue * sign; + isLimitingActive.set(true); + } + else + { + isLimitingActive.set(false); + } + + this.set(desiredDoubleValue); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java new file mode 100644 index 00000000..f93d052b --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java @@ -0,0 +1,201 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoEnum; + +/** + * @author jrebula + *

+ * A YoFilteredVelocityVariable is a filtered velocity of a position. + * Either a YoVariable holding the position is passed in to the + * constructor and update() is called every tick, or update(double) is + * called every tick. The YoFilteredVelocityVariable updates it's val + * with the current velocity after a filter of + *

+ * + * 
+ *            vel_{n} = alpha * vel{n-1} + (1 - alpha) * (pos_{n} - pos_{n-1})
+ *
+ * + */ +public class FilteredDiscreteVelocityYoVariable extends YoDouble +{ + private final double alpha; + + private final YoDouble time; + + private final YoDouble alphaVariable; + private final YoDouble position; + + private final YoDouble lastUpdateTime; + private final YoEnum lastUpdateDirection; + private final YoDouble unfilteredVelocity; + + private final YoDouble lastPosition; + private boolean hasBeenCalled; + + public FilteredDiscreteVelocityYoVariable(String name, String description, double alpha, YoDouble time, YoVariableRegistry registry) + { + super(name, description, registry); + + this.alpha = alpha; + this.alphaVariable = null; + this.position = null; + + this.time = time; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + lastUpdateTime = new YoDouble(name + "_lastUpdateTime", registry); + lastUpdateDirection = YoEnum.create(name + "_lastUpdateDirection", Direction.class, registry); + unfilteredVelocity = new YoDouble(name + "_unfilteredVelocity", registry); + + reset(); + } + + public FilteredDiscreteVelocityYoVariable(String name, String description, double alpha, YoDouble positionVariable, YoDouble time, + YoVariableRegistry registry) + { + super(name, description, registry); + + this.alpha = alpha; + this.position = positionVariable; + + this.alphaVariable = null; + + this.time = time; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + lastUpdateTime = new YoDouble(name + "_lastUpdateTime", registry); + lastUpdateDirection = YoEnum.create(name + "_lastUpdateDirection", Direction.class, registry); + unfilteredVelocity = new YoDouble(name + "_unfilteredVelocity", registry); + + reset(); + } + + public FilteredDiscreteVelocityYoVariable(String name, String description, YoDouble alphaVariable, YoDouble positionVariable, + YoDouble time, YoVariableRegistry registry) + { + super(name, description, registry); + position = positionVariable; + this.alphaVariable = alphaVariable; + this.alpha = 0.0; + + this.time = time; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + lastUpdateTime = new YoDouble(name + "_lastUpdateTime", registry); + lastUpdateDirection = YoEnum.create(name + "_lastUpdateDirection", Direction.class, registry); + unfilteredVelocity = new YoDouble(name + "_unfilteredVelocity", registry); + + reset(); + } + + public void reset() + { + hasBeenCalled = false; + } + + public void update() + { + if (position == null) + { + throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position.getDoubleValue()); + } + + public void update(double currentPosition) + { + if (!hasBeenCalled) + { + hasBeenCalled = true; + + // lastPosition = currentPosition; + lastPosition.set(currentPosition); + lastUpdateTime.set(time.getDoubleValue()); + lastUpdateDirection.set(Direction.NONE); + } + + double alphaToUse = alpha; + + if (alphaVariable != null) + { + alphaToUse = alphaVariable.getDoubleValue(); + } + + // Figure out if the count changed and if so, if the direction changed or not and then update the direction: + boolean countChanged = false; + if (currentPosition != lastPosition.getDoubleValue()) + countChanged = true; + + // If the count changed, figure out if the direction changed: + + boolean directionChanged = false; + if (countChanged) + { + if (currentPosition > lastPosition.getDoubleValue()) + { + if (lastUpdateDirection.getEnumValue() != Direction.FORWARD) + directionChanged = true; + lastUpdateDirection.set(Direction.FORWARD); + } + else if (currentPosition < lastPosition.getDoubleValue()) + { + if (lastUpdateDirection.getEnumValue() != Direction.BACKWARD) + directionChanged = true; + lastUpdateDirection.set(Direction.BACKWARD); + } + } + + // If the direction changed, then the velocity is set to zero: + if (directionChanged) + { + unfilteredVelocity.set(0.0); + } + + // If the direction hasn't changed, but the count changed then compute the velocity based on the time since last update: + else if (countChanged) + { + double diffTime = time.getDoubleValue() - lastUpdateTime.getDoubleValue(); + if (diffTime < 1e-7) + unfilteredVelocity.set(0.0); + else + { + unfilteredVelocity.set((currentPosition - lastPosition.getDoubleValue()) / diffTime); + } + } + + else + { + // If the count hasn't changed, then not quite sure what the velocity is. + // We could just use the current velocity, but really should try to figure out if things have been slowing down or not. + // For now, multiply by some largish fraction, just to make sure it does trail off to zero if the velocity stops quickly. + unfilteredVelocity.set(0.99 * unfilteredVelocity.getDoubleValue()); + } + + // Low pass alpha filter it... + set(alphaToUse * getDoubleValue() + (1.0 - alphaToUse) * unfilteredVelocity.getDoubleValue()); + + // Remember the position and the currentTime if the countChanged: + + if (countChanged) + { + lastPosition.set(currentPosition); + lastUpdateTime.set(time.getDoubleValue()); + } + } + + public double getUnfilteredVelocity() + { + return unfilteredVelocity.getDoubleValue(); + } + + private enum Direction + { + NONE, FORWARD, BACKWARD; + } + +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java new file mode 100644 index 00000000..70a895ad --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java @@ -0,0 +1,118 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.robotics.math.frames.YoFramePoint; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.robotics.math.frames.YoFrameVector; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +/** + *

FilteredVelocityYoFrameVector

+ * + *

Differentiates and Filters a YoFrameVector to get its derivative.

+ * + *

IHMC

+ * + * @author IHMC Biped Team + * @version 1.0 + */ +public class FilteredVelocityYoFrameVector extends YoFrameVector +{ + private final FilteredVelocityYoVariable xDot, yDot, zDot; + + public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, + YoVariableRegistry registry, YoFrameVector yoFrameVectorToDifferentiate) + { + FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, + yoFrameVectorToDifferentiate.getYoX(), dt, registry); + FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, + yoFrameVectorToDifferentiate.getYoY(), dt, registry); + FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, + yoFrameVectorToDifferentiate.getYoZ(), dt, registry); + + FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, + yoFrameVectorToDifferentiate.getReferenceFrame()); + + return ret; + } + + public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, + YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) + { + FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, + yoFramePointToDifferentiate.getYoX(), dt, registry); + FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, + yoFramePointToDifferentiate.getYoY(), dt, registry); + FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, + yoFramePointToDifferentiate.getYoZ(), dt, registry); + + FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, + yoFramePointToDifferentiate.getReferenceFrame()); + + return ret; + } + + public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, + YoVariableRegistry registry, ReferenceFrame referenceFrame) + { + FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, dt, registry); + FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, dt, registry); + FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, dt, registry); + + FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, referenceFrame); + + return ret; + } + + private FilteredVelocityYoFrameVector(FilteredVelocityYoVariable xDot, FilteredVelocityYoVariable yDot, FilteredVelocityYoVariable zDot, + YoDouble alpha, double dt, YoVariableRegistry registry, ReferenceFrame referenceFrame) + { + super(xDot, yDot, zDot, referenceFrame); + + this.xDot = xDot; + this.yDot = yDot; + this.zDot = zDot; + } + + private FilteredVelocityYoFrameVector(FilteredVelocityYoVariable xDot, FilteredVelocityYoVariable yDot, FilteredVelocityYoVariable zDot, + YoDouble alpha, double dt, YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) + { + super(xDot, yDot, zDot, yoFramePointToDifferentiate.getReferenceFrame()); + + this.xDot = xDot; + this.yDot = yDot; + this.zDot = zDot; + } + + public void update() + { + xDot.update(); + yDot.update(); + zDot.update(); + } + + public void update(Tuple3DReadOnly tuple) + { + xDot.update(tuple.getX()); + yDot.update(tuple.getY()); + zDot.update(tuple.getZ()); + } + + public void update(FrameTuple3DReadOnly frameTuple) + { + checkReferenceFrameMatch(frameTuple); + xDot.update(frameTuple.getX()); + yDot.update(frameTuple.getY()); + zDot.update(frameTuple.getZ()); + } + + public void reset() + { + xDot.reset(); + yDot.reset(); + zDot.reset(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java new file mode 100644 index 00000000..10a2a55f --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java @@ -0,0 +1,91 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; +import us.ihmc.robotics.math.frames.YoFramePoint; +import us.ihmc.robotics.math.frames.YoFrameTuple2d; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.robotics.math.frames.YoFrameVector2d; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class FilteredVelocityYoFrameVector2d extends YoFrameVector2d +{ + private final FilteredVelocityYoVariable xDot, yDot; + + public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, YoDouble alpha, double dt, + YoVariableRegistry registry, YoFrameTuple2d yoFrameVectorToDifferentiate) + { + return createFilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, "", alpha, dt, registry, yoFrameVectorToDifferentiate); + } + + public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoDouble alpha, + double dt, YoVariableRegistry registry, + YoFrameTuple2d yoFrameVectorToDifferentiate) + { + FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, alpha, + yoFrameVectorToDifferentiate.getYoX(), dt, registry); + FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, alpha, + yoFrameVectorToDifferentiate.getYoY(), dt, registry); + + FilteredVelocityYoFrameVector2d ret = new FilteredVelocityYoFrameVector2d(xDot, yDot, alpha, dt, registry, + yoFrameVectorToDifferentiate.getReferenceFrame()); + + return ret; + } + + public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, YoDouble alpha, double dt, + YoVariableRegistry registry, ReferenceFrame referenceFrame) + { + FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, dt, registry); + FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, dt, registry); + + FilteredVelocityYoFrameVector2d ret = new FilteredVelocityYoFrameVector2d(xDot, yDot, alpha, dt, registry, referenceFrame); + + return ret; + } + + private FilteredVelocityYoFrameVector2d(FilteredVelocityYoVariable xDot, FilteredVelocityYoVariable yDot, YoDouble alpha, double dt, + YoVariableRegistry registry, ReferenceFrame referenceFrame) + { + super(xDot, yDot, referenceFrame); + + this.xDot = xDot; + this.yDot = yDot; + } + + private FilteredVelocityYoFrameVector2d(FilteredVelocityYoVariable xDot, FilteredVelocityYoVariable yDot, YoDouble alpha, double dt, + YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) + { + super(xDot, yDot, yoFramePointToDifferentiate.getReferenceFrame()); + + this.xDot = xDot; + this.yDot = yDot; + } + + public void update() + { + xDot.update(); + yDot.update(); + } + + public void update(Vector2DReadOnly vector) + { + xDot.update(vector.getX()); + yDot.update(vector.getY()); + } + + public void update(FrameTuple2DReadOnly tupleToDifferentiate) + { + checkReferenceFrameMatch(tupleToDifferentiate); + xDot.update(tupleToDifferentiate.getX()); + yDot.update(tupleToDifferentiate.getY()); + } + + public void reset() + { + xDot.reset(); + yDot.reset(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java new file mode 100644 index 00000000..a47053db --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java @@ -0,0 +1,179 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.robotics.geometry.AngleTools; + +/** + * @author jrebula + *

+ * A YoFilteredVelocityVariable is a filtered velocity of a position. + * Either a YoVariable holding the position is passed in to the + * constructor and update() is called every tick, or update(double) is + * called every tick. The YoFilteredVelocityVariable updates it's val + * with the current velocity after a filter of + *

+ * + * 
+ *            vel_{n} = alpha * vel{n-1} + (1 - alpha) * (pos_{n} - pos_{n-1})
+ *
+ * + */ +public class FilteredVelocityYoVariable extends YoDouble implements ProcessingYoVariable +{ + private double alphaDouble; + private final double dt; + + private final YoDouble alphaVariable; + private final YoDouble position; + +// private double lastPosition; + private final YoDouble lastPosition; + private final YoBoolean hasBeenCalled; + + public FilteredVelocityYoVariable(String name, String description, double alpha, double dt, YoVariableRegistry registry) + { + super(name, description, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + + this.alphaDouble = alpha; + this.dt = dt; + + this.alphaVariable = null; + this.position = null; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + + reset(); + } + + public FilteredVelocityYoVariable(String name, String description, double alpha, YoDouble positionVariable, double dt, YoVariableRegistry registry) + { + super(name, description, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + + this.alphaDouble = alpha; + this.position = positionVariable; + this.dt = dt; + + this.alphaVariable = null; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + + + reset(); + } + + public FilteredVelocityYoVariable(String name, String description, YoDouble alphaVariable, YoDouble positionVariable, double dt, YoVariableRegistry registry) + { + super(name, description, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + + position = positionVariable; + this.alphaVariable = alphaVariable; + this.alphaDouble = 0.0; + + this.dt = dt; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + + reset(); + } + + public FilteredVelocityYoVariable(String name, String description, YoDouble alphaVariable, double dt, YoVariableRegistry registry) + { + super(name, description, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + + this.position = null; + this.alphaVariable = alphaVariable; + this.alphaDouble = 0.0; + + this.dt = dt; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (position == null) + { + throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position.getDoubleValue()); + } + + public void updateForAngles() + { + if (position == null) + { + throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + updateForAngles(position.getDoubleValue()); + } + + public void update(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + lastPosition.set(currentPosition); + set(0.0); + } + + double difference = currentPosition - lastPosition.getDoubleValue(); + + updateUsingDifference(difference); + + lastPosition.set(currentPosition); + } + + public void updateForAngles(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + lastPosition.set(currentPosition); + set(0.0); + } + + double difference = AngleTools.computeAngleDifferenceMinusPiToPi(currentPosition, lastPosition.getDoubleValue()); + + updateUsingDifference(difference); + + lastPosition.set(currentPosition); + } + + private void updateUsingDifference(double difference) + { + double previousFilteredDerivative = getDoubleValue(); + double currentRawDerivative = difference / dt; + + double alpha = alphaVariable == null ? alphaDouble : alphaVariable.getDoubleValue(); + set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); + } + + public void setAlpha(double alpha) + { + if (alphaVariable == null) + { + this.alphaDouble = alpha; + } + else + { + alphaVariable.set(alpha); + } + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java new file mode 100644 index 00000000..aac85152 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java @@ -0,0 +1,104 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.axisAngle.interfaces.AxisAngleReadOnly; +import us.ihmc.euclid.matrix.RotationMatrix; +import us.ihmc.euclid.matrix.interfaces.RotationMatrixReadOnly; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.robotics.math.frames.YoFrameQuaternion; +import us.ihmc.robotics.math.frames.YoFrameVector; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class FiniteDifferenceAngularVelocityYoFrameVector extends YoFrameVector +{ + private final YoFrameQuaternion orientation; + private final YoFrameQuaternion orientationPreviousValue; + + private final YoBoolean hasBeenCalled; + + private final RotationMatrix currentOrientationMatrix = new RotationMatrix(); + private final RotationMatrix previousOrientationMatrix = new RotationMatrix(); + private final RotationMatrix deltaOrientationMatrix = new RotationMatrix(); + private final AxisAngle deltaAxisAngle = new AxisAngle(); + + private final double dt; + + public FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, ReferenceFrame referenceFrame, double dt, YoVariableRegistry registry) + { + this(namePrefix, null, referenceFrame, dt, registry); + } + + public FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, YoFrameQuaternion orientationToDifferentiate, double dt, YoVariableRegistry registry) + { + this(namePrefix, orientationToDifferentiate, orientationToDifferentiate.getReferenceFrame(), dt, registry); + } + + private FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, YoFrameQuaternion orientationToDifferentiate, ReferenceFrame referenceFrame, double dt, YoVariableRegistry registry) + { + super(namePrefix, referenceFrame, registry); + + this.dt = dt; + + orientation = orientationToDifferentiate; + orientationPreviousValue = new YoFrameQuaternion(namePrefix + "_previous", referenceFrame, registry); + + hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled", registry); + hasBeenCalled.set(false); + } + + public void update() + { + if (orientation == null) + { + throw new NullPointerException("FiniteDifferenceAngularVelocityYoFrameVector must be constructed with a non null " + + "orientation variable to call update(), otherwise use update(FrameOrientation)"); + } + + currentOrientationMatrix.set(orientation); + update(currentOrientationMatrix); + } + + public void update(FrameQuaternionReadOnly currentOrientation) + { + checkReferenceFrameMatch(currentOrientation); + + currentOrientationMatrix.set(currentOrientation); + update(currentOrientationMatrix); + } + + public void update(QuaternionReadOnly currentOrientation) + { + currentOrientationMatrix.set(currentOrientation); + update(currentOrientationMatrix); + } + + public void update(AxisAngleReadOnly currentOrientation) + { + currentOrientationMatrix.set(currentOrientation); + update(currentOrientationMatrix); + } + + public void update(RotationMatrixReadOnly rotationMatrix) + { + if (!hasBeenCalled.getBooleanValue()) + { + orientationPreviousValue.set(rotationMatrix); + hasBeenCalled.set(true); + } + + if (rotationMatrix != currentOrientationMatrix) + currentOrientationMatrix.set(rotationMatrix); + previousOrientationMatrix.set(orientationPreviousValue); + deltaOrientationMatrix.set(currentOrientationMatrix); + deltaOrientationMatrix.multiplyTransposeOther(previousOrientationMatrix); + deltaAxisAngle.set(deltaOrientationMatrix); + + set(deltaAxisAngle.getX(), deltaAxisAngle.getY(), deltaAxisAngle.getZ()); + scale(deltaAxisAngle.getAngle() / dt); + + orientationPreviousValue.set(currentOrientationMatrix); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java new file mode 100644 index 00000000..636ba048 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java @@ -0,0 +1,68 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class FirstOrderBandPassFilteredYoVariable extends FirstOrderFilteredYoVariable +{ + private boolean hasBeenCalled = false; + + private final FirstOrderFilteredYoVariable highPassFilteredInput; + + public FirstOrderBandPassFilteredYoVariable(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz, + YoDouble yoTime, YoVariableRegistry registry) + { + super(name, description, maxPassThroughFrequency_Hz, yoTime, FirstOrderFilterType.LOW_PASS, registry); + + this.highPassFilteredInput = new FirstOrderFilteredYoVariable(name + "HighPassFilteredOnly", description, minPassThroughFrequency_Hz, yoTime, + FirstOrderFilterType.HIGH_PASS, registry); + + setPassBand(minPassThroughFrequency_Hz, maxPassThroughFrequency_Hz); + } + + public FirstOrderBandPassFilteredYoVariable(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz, + double DT, YoVariableRegistry registry) + { + super(name, description, maxPassThroughFrequency_Hz, DT, FirstOrderFilterType.LOW_PASS, registry); + + this.highPassFilteredInput = new FirstOrderFilteredYoVariable(name + "HighPassFilteredOnly", description, minPassThroughFrequency_Hz, DT, + FirstOrderFilterType.HIGH_PASS, registry); + } + + private void checkPassband(double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz) + { + if (minPassThroughFrequency_Hz > maxPassThroughFrequency_Hz) + { + throw new RuntimeException("minPassThroughFrequency [ " + minPassThroughFrequency_Hz + " ] > maxPassThroughFrequency [ " + maxPassThroughFrequency_Hz + + " ]"); + } + } + + public void update(double filterInput) + { + if (!hasBeenCalled) + { + hasBeenCalled = true; + this.set(filterInput); + } + else + { + updateHighPassFilterAndThenLowPassFilterThat(filterInput); + } + } + + public void setPassBand(double minPassThroughFreqHz, double maxPassThroughFreqHz) + { + checkPassband(minPassThroughFreqHz, maxPassThroughFreqHz); + + highPassFilteredInput.setCutoffFrequencyHz(minPassThroughFreqHz); + this.setCutoffFrequencyHz(maxPassThroughFreqHz); + } + + private void updateHighPassFilterAndThenLowPassFilterThat(double filterInput) + { + this.highPassFilteredInput.update(filterInput); + + super.update(highPassFilteredInput.getDoubleValue()); + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java new file mode 100644 index 00000000..d1c7f545 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java @@ -0,0 +1,157 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class FirstOrderFilteredYoVariable extends YoDouble + { + + public enum FirstOrderFilterType + { + LOW_PASS, HIGH_PASS + } + + private boolean hasBeenCalled = false; + + private double filterInputOld; + private double filterUpdateTimeOld; + + private final YoDouble cutoffFrequency_Hz; + + private final YoDouble yoTime; + private double dt; + + private FirstOrderFilterType highOrLowPass; + + public FirstOrderFilteredYoVariable(String name, String description, double cutoffFrequency_Hz, YoDouble yoTime, FirstOrderFilterType highOrLowPass, YoVariableRegistry registry) + { + super(name, description, registry); + + String cutoffFrequencyName; + switch (highOrLowPass) + { + case LOW_PASS: + cutoffFrequencyName = name + "_LowPassCutoff_Hz"; + break; + case HIGH_PASS: + cutoffFrequencyName = name + "_HighPassCutoff_Hz"; + break; + default: + throw new RuntimeException("Must Specify Filter Type as Low or High Pass. Current Specification : " + highOrLowPass); + } + + this.cutoffFrequency_Hz = new YoDouble(cutoffFrequencyName, registry); + this.cutoffFrequency_Hz.set(cutoffFrequency_Hz); + + this.yoTime = yoTime; + + this.highOrLowPass = highOrLowPass; + } + + public FirstOrderFilteredYoVariable(String name, String description, double cutoffFrequency_Hz, double DT, FirstOrderFilterType highOrLowPass, YoVariableRegistry registry) + { + this(name, description, cutoffFrequency_Hz, null, highOrLowPass, registry); + this.dt = DT; + } + + private double computeLowPassUpdate(double filterInput, double dt) + { + double alpha = computeAlpha( dt, cutoffFrequency_Hz.getDoubleValue()); + + double ret = alpha * this.getDoubleValue() + (1.0-alpha) * filterInput; + + return ret; + } + + private double computeHighPassUpdate(double filterInput, double dt) + { + double alpha = computeAlpha(dt, cutoffFrequency_Hz.getDoubleValue()); + + double ret = alpha * (this.getDoubleValue() + filterInput - filterInputOld); + return ret; + } + + private double computeAlpha(double dt, double cutoffFrequencyHz) + { + if (cutoffFrequencyHz <= 0.0) + { + throw new RuntimeException("Cutoff Frequency must be greater than zero. Cutoff = " + cutoffFrequencyHz); + } + + double cutoff_radPerSec = cutoffFrequencyHz * 2.0 * Math.PI; + double RC = 1.0 / cutoff_radPerSec; + double alpha = RC / (RC + dt); // alpha decreases with increasing cutoff frequency + + if (alpha <= 0 || alpha >= 1.0 && dt != 0.0) + { + throw new RuntimeException("Alpha value must be between 0 and 1. Alpha = " + alpha); + } + + return alpha; + } + + public void reset() + { + hasBeenCalled = false; + } + + public void setCutoffFrequencyHz(double cutoffHz) + { + this.cutoffFrequency_Hz.set(cutoffHz); + } + + public void update(double filterInput) + { + if (!hasBeenCalled) + { + hasBeenCalled = true; + + filterInputOld = 0.0; + filterUpdateTimeOld = 0.0; + + this.set(filterInput); + } + else + { + if ( yoTime != null ) + { + double timeSinceLastUpdate = yoTime.getDoubleValue() - filterUpdateTimeOld; + + if (timeSinceLastUpdate > 0.0) + { + dt = timeSinceLastUpdate; + } + else + { + reset(); +// throw new RuntimeException("Computed step size, DT must be greater than zero. DT = " + dt + ". Current time = " + yoTime.getDoubleValue() + ", previous update time = " + filterUpdateTimeOld); + } + } + + double filterOutput; + + switch (highOrLowPass) + { + case LOW_PASS: + filterOutput = computeLowPassUpdate(filterInput, dt); + break; + case HIGH_PASS: + filterOutput = computeHighPassUpdate(filterInput, dt); + break; + + default: + filterOutput = filterInput; + break; + } + + this.set(filterOutput); + } + + filterInputOld = filterInput; + + if (yoTime != null) + { + filterUpdateTimeOld = yoTime.getDoubleValue(); + } + } + } \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java new file mode 100644 index 00000000..1cbfaaad --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java @@ -0,0 +1,110 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoInteger; + +public class GlitchFilteredYoBoolean extends YoBoolean +{ + private YoBoolean variableToFilter; + private final YoInteger windowSize; + protected final YoInteger counter; + + public GlitchFilteredYoBoolean(String name, int windowSize) + { + super(name, "GlitchFilteredYoBoolean", null); + counter = new YoInteger(name + "Count", getYoVariableRegistry()); + this.windowSize = new YoInteger(name + "WindowSize", getYoVariableRegistry()); + + initialize(null, windowSize); + } + + public GlitchFilteredYoBoolean(String name, YoVariableRegistry registry, int windowSize) + { + super(name, registry); + counter = new YoInteger(name + "Count", registry); + this.windowSize = new YoInteger(name + "WindowSize", registry); + + initialize(null, windowSize); + } + + public GlitchFilteredYoBoolean(String name, YoBoolean yoVariableToFilter, int windowSize) + { + super(name, "GlitchFilteredYoBoolean", null); + counter = new YoInteger(name + "Count", getYoVariableRegistry()); + this.windowSize = new YoInteger(name + "WindowSize", getYoVariableRegistry()); + + initialize(yoVariableToFilter, windowSize); + } + + public GlitchFilteredYoBoolean(String name, YoVariableRegistry registry, YoBoolean yoVariableToFilter, int windowSize) + { + this(name, "", registry, yoVariableToFilter, windowSize); + } + + public GlitchFilteredYoBoolean(String name, String description, YoVariableRegistry registry, YoBoolean yoVariableToFilter, int windowSize) + { + super(name, description, registry); + counter = new YoInteger(name + "Count", description, registry); + this.windowSize = new YoInteger(name + "WindowSize", description, registry); + + initialize(yoVariableToFilter, windowSize); + } + + private void initialize(YoBoolean yoVariableToFilter, int windowSize) + { + if (windowSize < 0) + throw new RuntimeException("window size must be greater than 0"); + + variableToFilter = yoVariableToFilter; + this.windowSize.set(windowSize); + + if (variableToFilter != null) + this.set(yoVariableToFilter.getBooleanValue()); + + this.set(false); + } + + public void set(boolean value) + { + super.set(value); + if (counter != null) + { + counter.set(0); + } + } + + public void update(boolean value) + { + + if (value != this.getBooleanValue()) + { + counter.set(counter.getIntegerValue() + 1); + } + else + counter.set(0); + + if (counter.getIntegerValue() >= (windowSize.getIntegerValue() )) + set(value); + } + + public int getWindowSize() + { + return windowSize.getIntegerValue(); + } + + public void setWindowSize(int windowSize) //untested + { + this.windowSize.set(windowSize); + } + + public void update() + { + if (variableToFilter == null) + { + throw new RuntimeException("variableToFilter was not initialized. Use the other constructor"); + } + else + update(variableToFilter.getBooleanValue()); + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java new file mode 100644 index 00000000..92fa4f63 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java @@ -0,0 +1,82 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoInteger; + +public class GlitchFilteredYoInteger extends YoInteger +{ + private final YoInteger position; + private final YoInteger previousPosition; + private final YoInteger windowSize; + private final YoInteger counter; + + public GlitchFilteredYoInteger(String name, int windowSize, YoVariableRegistry registry) + { + this(name, windowSize, null, registry); + } + + public GlitchFilteredYoInteger(String name, int windowSize, YoInteger position, YoVariableRegistry registry) + { + super(name, GlitchFilteredYoInteger.class.getSimpleName(), registry); + + this.position = position; + + previousPosition = new YoInteger(name + "PrevValue", registry); + counter = new YoInteger(name + "Count", registry); + this.windowSize = new YoInteger(name + "WindowSize", registry); + this.windowSize.set(windowSize); + } + + @Override + public void set(int value) + { + super.set(value); + if (counter != null) + counter.set(0); + } + + @Override + public boolean set(int value, boolean notifyListeners) + { + if (counter != null) + counter.set(0); + return super.set(value, notifyListeners); + } + + public void update() + { + if (position == null) + { + throw new NullPointerException( + "GlitchFilteredYoInteger must be constructed with a non null position variable to call update(), otherwise use update(int)"); + } + + update(position.getIntegerValue()); + } + + public void update(int currentValue) + { + if (currentValue == previousPosition.getIntegerValue()) + counter.increment(); + else + counter.set(0); + + if (counter.getIntegerValue() >= windowSize.getIntegerValue()) + { + set(currentValue); + counter.set(0); + } + + previousPosition.set(currentValue); + } + + public int getWindowSize() + { + return windowSize.getIntegerValue(); + } + + public void setWindowSize(int windowSize) + { + this.windowSize.set(windowSize); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java new file mode 100644 index 00000000..556f17b8 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java @@ -0,0 +1,151 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class JerkLimitedYoVariable extends YoDouble +{ + private final double dt; + + private final YoBoolean hasBeenInitialized; + + private final YoDouble smoothedRate; + private final YoDouble smoothedAcceleration; + private final YoDouble smoothedJerk; + + private final YoDouble positionGain; + private final YoDouble velocityGain; + private final YoDouble accelerationGain; + + private final YoDouble maximumJerk; + private final YoDouble maximumAcceleration; + + private final YoDouble inputPosition; + private final YoDouble inputVelocity; + private final YoDouble inputAcceleration; + + public JerkLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, double dt) + { + this(name, registry, maxAcceleration, maxJerk, null, null, null, dt); + } + + public JerkLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, + YoDouble inputPosition, double dt) + { + this(name, registry, maxAcceleration, maxJerk, inputPosition, null, null, dt); + } + + public JerkLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, + YoDouble inputPosition, YoDouble inputVelocity, double dt) + { + this(name, registry, maxAcceleration, maxJerk, inputPosition, inputVelocity, null, dt); + } + + public JerkLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, + YoDouble inputPosition, YoDouble inputVelocity, YoDouble inputAcceleration, double dt) + { + super(name, registry); + + this.inputPosition = inputPosition; + this.inputVelocity = inputVelocity; + this.inputAcceleration = inputAcceleration; + + this.maximumJerk = maxJerk; + this.maximumAcceleration = maxAcceleration; + + this.dt = dt; + + hasBeenInitialized = new YoBoolean(name + "HasBeenInitialized", registry); + + smoothedRate = new YoDouble(name + "SmoothedRate", registry); + smoothedAcceleration = new YoDouble(name + "SmoothedAcceleration", registry); + smoothedJerk = new YoDouble(name + "SmoothedJerk", registry); + + positionGain = new YoDouble(name + "PositionGain", registry); + velocityGain = new YoDouble(name + "VelocityGain", registry); + accelerationGain = new YoDouble(name + "AccelerationGain", registry); + + double w0 = 2.0 * Math.PI * 16.0; + double w1 = 2.0 * Math.PI * 16.0; + double zeta = 1.0; + + setGainsByPolePlacement(w0, w1, zeta); + hasBeenInitialized.set(false); + } + + public void setMaximumAcceleration(double maximumAcceleration) + { + this.maximumAcceleration.set(maximumAcceleration); + } + + public void setMaximumJerk(double maximumJerk) + { + this.maximumJerk.set(maximumJerk); + } + + public void setGainsByPolePlacement(double w0, double w1, double zeta) + { + positionGain.set(w0 * w1 * w1); + velocityGain.set(w1 * w1 + 2.0 * zeta * w1 * w0); + accelerationGain.set(w0 + 2.0 * zeta * w1); + } + + public void update() + { + double inputPosition = this.inputPosition == null ? 0.0 : this.inputPosition.getDoubleValue(); + double inputVelocity = this.inputVelocity == null ? 0.0 : this.inputVelocity.getDoubleValue(); + double inputAcceleration = this.inputAcceleration == null ? 0.0 : this.inputAcceleration.getDoubleValue(); + + update(inputPosition, inputVelocity, inputAcceleration); + } + + public void update(double inputPosition) + { + update(inputPosition, 0.0, 0.0); + } + + public void update(double inputPosition, double inputVelocity) + { + update(inputPosition, inputVelocity, 0.0); + } + + public void update(double inputPosition, double inputVelocity, double inputAcceleration) + { + if (!hasBeenInitialized.getBooleanValue()) + initialize(inputPosition, inputVelocity, inputAcceleration); + + double positionError = inputPosition - this.getDoubleValue(); + double velocityError = inputVelocity - smoothedRate.getDoubleValue(); + double accelerationError = inputAcceleration - smoothedAcceleration.getDoubleValue(); + double jerk = accelerationGain.getDoubleValue() * accelerationError + velocityGain.getDoubleValue() * velocityError + positionGain.getDoubleValue() + * positionError; + jerk = MathTools.clamp(jerk, -maximumJerk.getDoubleValue(), maximumJerk.getDoubleValue()); + + smoothedJerk.set(jerk); + smoothedAcceleration.add(smoothedJerk.getDoubleValue() * dt); + smoothedAcceleration.set(MathTools.clamp(smoothedAcceleration.getDoubleValue(), maximumJerk.getDoubleValue())); + smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); + this.add(smoothedRate.getDoubleValue() * dt); + } + + public void initialize(double inputPosition, double inputVelocity, double inputAcceleration) + { + this.set(inputPosition); + smoothedRate.set(inputVelocity); + smoothedAcceleration.set(inputAcceleration); + smoothedJerk.set(0.0); + + this.hasBeenInitialized.set(true); + } + + public void reset() + { + this.hasBeenInitialized.set(false); + smoothedRate.set(0.0); + smoothedAcceleration.set(0.0); + smoothedJerk.set(0.0); + } + +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java new file mode 100644 index 00000000..7353f617 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java @@ -0,0 +1,6 @@ +package us.ihmc.robotics.math.filters; + +public interface ProcessingYoVariable +{ + public abstract void update(); +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java new file mode 100644 index 00000000..190ff7de --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java @@ -0,0 +1,124 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FrameQuaternion; +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.robotics.math.frames.YoFrameOrientation; + +public class RateLimitedYoFrameOrientation extends YoFrameOrientation +{ + private final YoDouble maxRateVariable; + + private final YoFrameOrientation rawOrientation; + private final YoBoolean limited; + private final YoBoolean hasBeenCalled; + private final double dt; + + private final FrameVector3D differenceVector = new FrameVector3D(); + + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, + YoFrameOrientation rawOrientation) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawOrientation, rawOrientation.getReferenceFrame()); + } + + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); + } + + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, YoFrameOrientation rawOrientation) + { + this(namePrefix, nameSuffix, registry, null, dt, rawOrientation, rawOrientation.getReferenceFrame()); + setMaxRate(maxRate); + } + + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, null, dt, null, referenceFrame); + setMaxRate(maxRate); + } + + private RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, + YoFrameOrientation rawOrientation, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); + this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); + + if (maxRate != null) + this.maxRateVariable = maxRate; + else + this.maxRateVariable = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + + this.rawOrientation = rawOrientation; + + this.dt = dt; + + reset(); + } + + public void setMaxRate(double maxRate) + { + this.maxRateVariable.set(maxRate); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawOrientation == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawOrientation); + } + + public void update(YoFrameOrientation yoFrameVectorUnfiltered) + { + checkReferenceFrameMatch(yoFrameVectorUnfiltered); + update(yoFrameVectorUnfiltered.getYaw().getDoubleValue(), yoFrameVectorUnfiltered.getPitch().getDoubleValue(), + yoFrameVectorUnfiltered.getRoll().getDoubleValue()); + } + + public void update(FrameQuaternion frameOrientationUnfiltered) + { + checkReferenceFrameMatch(frameOrientationUnfiltered); + update(frameOrientationUnfiltered); + } + + public void update(QuaternionReadOnly quaternionUnfiltered) + { + update(quaternionUnfiltered.getYaw(), quaternionUnfiltered.getPitch(), quaternionUnfiltered.getRoll()); + } + + public void update(double yawUnfiltered, double pitchUnfiltered, double rollUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + setYawPitchRoll(yawUnfiltered, pitchUnfiltered, rollUnfiltered); + } + + if (maxRateVariable.getDoubleValue() < 0) + throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); + + differenceVector.setToZero(getReferenceFrame()); + differenceVector.set(yawUnfiltered, pitchUnfiltered, rollUnfiltered); + differenceVector.sub(getYaw().getDoubleValue(), getPitch().getDoubleValue(), getRoll().getDoubleValue()); + + limited.set(differenceVector.clipToMaxLength(maxRateVariable.getDoubleValue() * dt)); + add(differenceVector.getX(), differenceVector.getY(), differenceVector.getZ()); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java new file mode 100644 index 00000000..286207ad --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java @@ -0,0 +1,125 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DBasics; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.robotics.math.frames.YoFrameTuple; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.robotics.math.frames.YoFrameVector; + +public class RateLimitedYoFrameVector extends YoFrameVector +{ + private final YoDouble maxRateVariable; + + private final YoFrameTuple rawPosition; + private final YoBoolean limited; + private final YoBoolean hasBeenCalled; + private final double dt; + + private final FrameVector3D differenceVector = new FrameVector3D(); + + public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, + YoFrameTuple rawPosition) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); + } + + public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); + } + + public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, YoFrameTuple rawPosition) + { + this(namePrefix, nameSuffix, registry, null, dt, rawPosition, rawPosition.getReferenceFrame()); + setMaxRate(maxRate); + } + + public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, null, dt, null, referenceFrame); + setMaxRate(maxRate); + } + + private RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, + YoFrameTuple rawPosition, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); + this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); + + if (maxRate != null) + this.maxRateVariable = maxRate; + else + this.maxRateVariable = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + + this.rawPosition = rawPosition; + + this.dt = dt; + + reset(); + } + + public void setMaxRate(double maxRate) + { + this.maxRateVariable.set(maxRate); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawPosition); + } + + public void update(YoFrameTuple yoFrameVectorUnfiltered) + { + checkReferenceFrameMatch(yoFrameVectorUnfiltered); + update(yoFrameVectorUnfiltered.getX(), yoFrameVectorUnfiltered.getY(), yoFrameVectorUnfiltered.getZ()); + } + + public void update(FrameTuple3DBasics frameVectorUnfiltered) + { + checkReferenceFrameMatch(frameVectorUnfiltered); + update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); + } + + public void update(Tuple3DReadOnly vectorUnfiltered) + { + update(vectorUnfiltered.getX(), vectorUnfiltered.getY(), vectorUnfiltered.getZ()); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered, zUnfiltered); + } + + if (maxRateVariable.getDoubleValue() < 0) + throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); + + differenceVector.setToZero(getReferenceFrame()); + differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); + differenceVector.sub(getX(), getY(), getZ()); + + limited.set(differenceVector.clipToMaxLength(maxRateVariable.getDoubleValue() * dt)); + add(differenceVector); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java new file mode 100644 index 00000000..a6e1833f --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java @@ -0,0 +1,107 @@ +package us.ihmc.robotics.math.filters; + + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.robotics.math.frames.YoFrameVector2d; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + + +public class RateLimitedYoFrameVector2d extends YoFrameVector2d +{ + private final RateLimitedYoVariable x, y; + + private RateLimitedYoFrameVector2d(RateLimitedYoVariable x, RateLimitedYoVariable y, ReferenceFrame referenceFrame) + { + super(x, y, referenceFrame); + + this.x = x; + this.y = y; + } + + public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + double maxRate, double dt, ReferenceFrame referenceFrame) + { + RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); + RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); + + RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, referenceFrame); + + return ret; + } + + public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + YoDouble maxRate, double dt, ReferenceFrame referenceFrame) + { + RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); + RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); + + RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, referenceFrame); + + return ret; + } + + + public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + double maxRate, double dt, YoFrameVector2d unfilteredVector) + { + RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); + RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); + + RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); + + return ret; + } + + + public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + YoDouble maxRate, double dt, YoFrameVector2d unfilteredVector) + { + RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); + RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); + + RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); + + return ret; + } + + public void update() + { + x.update(); + y.update(); + } + + public void update(double xUnfiltered, double yUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + } + + public void update(Vector2DReadOnly vector2dUnfiltered) + { + x.update(vector2dUnfiltered.getX()); + y.update(vector2dUnfiltered.getY()); + } + + public void update(FrameTuple2DReadOnly vector2dUnfiltered) + { + x.update(vector2dUnfiltered.getX()); + y.update(vector2dUnfiltered.getY()); + } + + public void setMaxRate(double maxRate) + { + x.setMaxRate(maxRate); + y.setMaxRate(maxRate); + } + + public void reset() + { + x.reset(); + y.reset(); + } +} + diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java new file mode 100644 index 00000000..35f4a687 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java @@ -0,0 +1,120 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class RateLimitedYoVariable extends YoDouble +{ + private final YoDouble maxRateVariable; + + private final YoDouble position; + private final YoBoolean limited; + + private final double dt; + + private final YoBoolean hasBeenCalled; + + public RateLimitedYoVariable(String name, YoVariableRegistry registry, double maxRate, double dt) + { + super(name, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + this.limited = new YoBoolean(name + "Limited", registry); + + this.maxRateVariable = new YoDouble(name + "MaxRate", registry); + this.maxRateVariable.set(maxRate); + + this.position = null; + + this.dt = dt; + + reset(); + } + + public RateLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxRateVariable, double dt) + { + super(name, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + this.limited = new YoBoolean(name + "Limited", registry); + + this.maxRateVariable = maxRateVariable; + this.position = null; + + this.dt = dt; + reset(); + } + + public RateLimitedYoVariable(String name, YoVariableRegistry registry, double maxRate, YoDouble positionVariable, double dt) + { + super(name, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + this.limited = new YoBoolean(name + "Limited", registry); + + position = positionVariable; + + this.maxRateVariable = new YoDouble(name + "MaxRate", registry); + this.maxRateVariable.set(maxRate); + + this.dt = dt; + + reset(); + } + + public RateLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxRateVariable, YoDouble positionVariable, double dt) + { + super(name, registry); + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + this.limited = new YoBoolean(name + "Limited", registry); + + position = positionVariable; + this.maxRateVariable = maxRateVariable; + + this.dt = dt; + + reset(); + } + + public void setMaxRate(double maxRate) + { + this.maxRateVariable.set(maxRate); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (position == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position.getDoubleValue()); + } + + public void update(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + set(currentPosition); + } + + if (maxRateVariable.getDoubleValue() < 0) + throw new RuntimeException("The maxRate parameter in the RateLimitedYoVariable cannot be negative."); + + double difference = currentPosition - getDoubleValue(); + if (Math.abs(difference) > maxRateVariable.getDoubleValue() * dt) + { + difference = Math.signum(difference) * maxRateVariable.getDoubleValue() * dt; + this.limited.set(true); + } + else + this.limited.set(false); + + set(getDoubleValue() + difference); + } +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java new file mode 100644 index 00000000..e1952293 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java @@ -0,0 +1,213 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoEnum; + +/** + * This does essentially the same as BacklashCompensatingVelocityYoVariable, except does the steps in a different order and + * goes sloppy when the output changes sign rather than the finite differences. This works better for a signal with noise on it, + * which might have a true velocity but due to the noise seems to switch direction often. + * + */ +public class RevisedBacklashCompensatingVelocityYoVariable extends YoDouble implements ProcessingYoVariable +{ + private final double dt; + + private final FilteredVelocityYoVariable finiteDifferenceVelocity; + + private final YoDouble alphaVariable; + private final YoDouble position; + + private final YoDouble lastPosition; + private final YoBoolean hasBeenCalled; + + private final YoEnum backlashState; + private final YoDouble slopTime; + + private final YoDouble timeSinceSloppy; + + public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, YoDouble positionVariable, + double dt, YoDouble slopTime, YoVariableRegistry registry) + { + super(name, description, registry); + + finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, registry); + + + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + + backlashState = new YoEnum(name + "BacklashState", registry, BacklashState.class, true); + backlashState.set(null); + timeSinceSloppy = new YoDouble(name + "TimeSinceSloppy", registry); + + position = positionVariable; + + this.alphaVariable = alphaVariable; + this.slopTime = slopTime; + + this.dt = dt; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + + reset(); + } + + public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, double dt, YoDouble slopTime, + YoVariableRegistry registry) + { + super(name, description, registry); + + finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, registry); + + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + + backlashState = new YoEnum(name + "BacklashState", registry, BacklashState.class, true); + backlashState.set(null); + timeSinceSloppy = new YoDouble(name + "timeInState", registry); + + this.position = null; + + this.alphaVariable = alphaVariable; + this.slopTime = slopTime; + + this.dt = dt; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + backlashState.set(null); + } + + public void update() + { + if (position == null) + { + throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position.getDoubleValue()); + } + + public void update(double currentPosition) + { + if (backlashState.getEnumValue() == null) + { + backlashState.set(BacklashState.FORWARD_OK); + } + + finiteDifferenceVelocity.update(currentPosition); + + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + lastPosition.set(currentPosition); + set(0.0); + } + + timeSinceSloppy.add(dt); + + double velocityFromFiniteDifferences = finiteDifferenceVelocity.getDoubleValue(); + + switch (backlashState.getEnumValue()) + { + case BACKWARD_OK : + { + if (velocityFromFiniteDifferences > 0.0) + { + timeSinceSloppy.set(0.0); +// this.set(0.0); + backlashState.set(BacklashState.FORWARD_SLOP); + } + + break; + } + + case FORWARD_OK : + { + if (velocityFromFiniteDifferences < 0.0) + { + timeSinceSloppy.set(0.0); +// this.set(0.0); + backlashState.set(BacklashState.BACKWARD_SLOP); + } + + break; + } + + case BACKWARD_SLOP : + { + if (velocityFromFiniteDifferences > 0.0) + { + timeSinceSloppy.set(0.0); +// this.set(0.0); + backlashState.set(BacklashState.FORWARD_SLOP); + } + else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) + { + backlashState.set(BacklashState.BACKWARD_OK); + } + + break; + } + + case FORWARD_SLOP : + { + if (velocityFromFiniteDifferences < 0.0) + { + timeSinceSloppy.set(0.0); +// this.set(0.0); + backlashState.set(BacklashState.BACKWARD_SLOP); + } + else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) + { + backlashState.set(BacklashState.FORWARD_OK); + } + + break; + } + } + + double difference = currentPosition - lastPosition.getDoubleValue(); + + double percent = timeSinceSloppy.getDoubleValue() / slopTime.getDoubleValue(); + percent = MathTools.clamp(percent, 0.0, 1.0); + if (Double.isNaN(percent)) + percent = 1.0; + + double scaleFactor = percent; + difference = scaleFactor * difference; + + this.updateUsingDifference(difference); + lastPosition.set(currentPosition); + } + + private void updateUsingDifference(double difference) + { + double previousFilteredDerivative = getDoubleValue(); + double currentRawDerivative = difference / dt; + + double alpha = alphaVariable.getDoubleValue(); + this.set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); + } + + public void setAlpha(double alpha) + { + this.alphaVariable.set(alpha); + } + + public void setSlopTime(double slopTime) + { + this.slopTime.set(slopTime); + } + + private enum BacklashState {BACKWARD_OK, FORWARD_OK, BACKWARD_SLOP, FORWARD_SLOP;} +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java new file mode 100644 index 00000000..92ba898a --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java @@ -0,0 +1,95 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.robotics.math.frames.YoFrameVector; + +public class SecondOrderFilteredYoFrameVector extends YoFrameVector implements ProcessingYoVariable +{ + private final SecondOrderFilteredYoVariable x, y, z; + + private SecondOrderFilteredYoFrameVector(SecondOrderFilteredYoVariable x, SecondOrderFilteredYoVariable y, SecondOrderFilteredYoVariable z, + ReferenceFrame referenceFrame) + { + super(x, y, z, referenceFrame); + + this.x = x; + this.y = y; + this.z = z; + } + + public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType, ReferenceFrame referenceFrame) + { + SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, registry, naturalFrequencyInHz, + dampingRatio, filterType); + return createSecondOrderFilteredYoFrameVector(namePrefix, nameSuffix, registry, dt, parameters, referenceFrame); + } + + public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + double dt, SecondOrderFilteredYoVariableParameters parameters, ReferenceFrame referenceFrame) + { + SecondOrderFilteredYoVariable x, y, z; + x = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, dt, parameters); + y = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, dt, parameters); + z = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, dt, parameters); + return new SecondOrderFilteredYoFrameVector(x, y, z, referenceFrame); + } + + public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType, YoFrameVector unfilteredVector) + { + SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, registry, naturalFrequencyInHz, + dampingRatio, filterType); + return createSecondOrderFilteredYoFrameVector(namePrefix, nameSuffix, registry, dt, parameters, unfilteredVector); + } + + public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + double dt, SecondOrderFilteredYoVariableParameters parameters, YoFrameVector unfilteredVector) + { + SecondOrderFilteredYoVariable x, y, z; + x = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoX()); + y = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoY()); + z = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoZ()); + return new SecondOrderFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); + } + + public void update() + { + x.update(); + y.update(); + z.update(); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + z.update(zUnfiltered); + } + + public void update(Vector3D vectorUnfiltered) + { + x.update(vectorUnfiltered.getX()); + y.update(vectorUnfiltered.getY()); + z.update(vectorUnfiltered.getZ()); + } + + public void update(FrameVector3D vectorUnfiltered) + { + checkReferenceFrameMatch(vectorUnfiltered); + x.update(vectorUnfiltered.getX()); + y.update(vectorUnfiltered.getY()); + z.update(vectorUnfiltered.getZ()); + } + + public void reset() + { + x.reset(); + y.reset(); + z.reset(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java new file mode 100644 index 00000000..8c3cac60 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java @@ -0,0 +1,36 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class SecondOrderFilteredYoVariableParameters +{ + private final YoDouble naturalFrequencyInHz; + private final YoDouble dampingRatio; + private final SecondOrderFilterType filterType; + + public SecondOrderFilteredYoVariableParameters(String name, YoVariableRegistry registry, double naturalFrequencyInHz, double dampingRatio, + SecondOrderFilterType filterType) + { + this.naturalFrequencyInHz = new YoDouble(name + "NaturalFrequency", registry); + this.naturalFrequencyInHz.set(naturalFrequencyInHz); + this.dampingRatio = new YoDouble(name + "DampingRatio", registry); + this.dampingRatio.set(dampingRatio); + this.filterType = filterType; + } + + public YoDouble getNaturalFrequencyInHz() + { + return naturalFrequencyInHz; + } + + public YoDouble getDampingRatio() + { + return dampingRatio; + } + + public SecondOrderFilterType getFilterType() + { + return filterType; + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java new file mode 100644 index 00000000..e2550a48 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java @@ -0,0 +1,95 @@ +package us.ihmc.robotics.math.filters; + +import static us.ihmc.robotics.math.frames.YoFrameVariableNameTools.createXName; +import static us.ihmc.robotics.math.frames.YoFrameVariableNameTools.createYName; +import static us.ihmc.robotics.math.frames.YoFrameVariableNameTools.createZName; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.robotics.math.frames.YoFrameVector; + +public class SimpleMovingAverageFilteredYoFrameVector extends YoFrameVector implements ProcessingYoVariable +{ + private final SimpleMovingAverageFilteredYoVariable x, y, z; + + private SimpleMovingAverageFilteredYoFrameVector(SimpleMovingAverageFilteredYoVariable x, SimpleMovingAverageFilteredYoVariable y, + SimpleMovingAverageFilteredYoVariable z, ReferenceFrame referenceFrame) + { + super(x, y, z, referenceFrame); + + this.x = x; + this.y = y; + this.z = z; + } + + public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, + ReferenceFrame referenceFrame, YoVariableRegistry registry) + { + String xName = createXName(namePrefix, nameSuffix); + String yName = createYName(namePrefix, nameSuffix); + String zName = createZName(namePrefix, nameSuffix); + + SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, registry); + SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, registry); + SimpleMovingAverageFilteredYoVariable z = new SimpleMovingAverageFilteredYoVariable(zName, windowSize, registry); + + return new SimpleMovingAverageFilteredYoFrameVector(x, y, z, referenceFrame); + } + + public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, + YoFrameVector unfilteredVector, YoVariableRegistry registry) + { + String xName = createXName(namePrefix, nameSuffix); + String yName = createYName(namePrefix, nameSuffix); + String zName = createZName(namePrefix, nameSuffix); + + SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, unfilteredVector.getYoX(), registry); + SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, unfilteredVector.getYoY(), registry); + SimpleMovingAverageFilteredYoVariable z = new SimpleMovingAverageFilteredYoVariable(zName, windowSize, unfilteredVector.getYoZ(), registry); + + return new SimpleMovingAverageFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); + } + + public void update() + { + x.update(); + y.update(); + z.update(); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + z.update(zUnfiltered); + } + + public void update(Vector3D vectorUnfiltered) + { + x.update(vectorUnfiltered.getX()); + y.update(vectorUnfiltered.getY()); + z.update(vectorUnfiltered.getZ()); + } + + public void update(FrameVector3D vectorUnfiltered) + { + checkReferenceFrameMatch(vectorUnfiltered); + x.update(vectorUnfiltered.getX()); + y.update(vectorUnfiltered.getY()); + z.update(vectorUnfiltered.getZ()); + } + + public void reset() + { + x.reset(); + y.reset(); + z.reset(); + } + + public boolean getHasBufferWindowFilled() + { + return x.getHasBufferWindowFilled() && y.getHasBufferWindowFilled() && z.getHasBufferWindowFilled(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java new file mode 100644 index 00000000..b4134e6b --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java @@ -0,0 +1,91 @@ +package us.ihmc.robotics.math.filters; + +import org.ejml.data.DenseMatrix64F; +import org.ejml.ops.CommonOps; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoInteger; + +/** + * Filter the given yoVariable using a moving average filter. + * + * + * This class is NOT REWINDABLE! + * + */ +public class SimpleMovingAverageFilteredYoVariable extends YoDouble +{ + private final YoInteger windowSize; + private final YoDouble yoVariableToFilter; + + private final DenseMatrix64F previousUpdateValues = new DenseMatrix64F(0, 0); + private int bufferPosition = 0; + + private boolean bufferHasBeenFilled = false; + + public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoVariableRegistry registry) + { + this(name, windowSize, null, registry); + } + + public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoDouble yoVariableToFilter, YoVariableRegistry registry) + { + super(name, registry); + + this.yoVariableToFilter = yoVariableToFilter; + this.windowSize = new YoInteger(name + "WindowSize", registry); + this.windowSize.set(windowSize); + + previousUpdateValues.reshape(windowSize, 1); + CommonOps.fill(previousUpdateValues, 0.0); + } + + public void setWindowSize(int windowSize) + { + this.windowSize.set(windowSize); + reset(); + } + + public void update() + { + update(yoVariableToFilter.getDoubleValue()); + } + + public void update(double value) + { + if (previousUpdateValues.getNumRows() != windowSize.getIntegerValue()) + { + reset(); + } + previousUpdateValues.set(bufferPosition, 0, value); + + bufferPosition++; + + if (bufferPosition >= windowSize.getIntegerValue()) + { + bufferPosition = 0; + bufferHasBeenFilled = true; + } + + double average = 0; + for (int i = 0; i < windowSize.getIntegerValue(); i++) + { + average += previousUpdateValues.get(i, 0); + } + + this.set(average / windowSize.getIntegerValue()); + } + + public void reset() + { + bufferPosition = 0; + bufferHasBeenFilled = false; + previousUpdateValues.reshape(windowSize.getIntegerValue(), 1); + } + + public boolean getHasBufferWindowFilled() + { + return bufferHasBeenFilled; + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java new file mode 100644 index 00000000..253c3fa3 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java @@ -0,0 +1,134 @@ +package us.ihmc.robotics.math.frames; + +import org.ejml.data.DenseMatrix64F; + +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoInteger; + +/** + * YoMatrix. Object for holding a matrix of YoVariables so that Matrices can be rewound. + * Has a maximum number of rows and columns and an actual number of rows and columns. + * If you set with a smaller matrix, then the actual size will be the size of the + * passed in matrix. extra + * entries will be set to NaN. If you get the contents the matrix you pack must be the + * correct size. + * @author JerryPratt + * + */ +public class YoMatrix +{ + private final int maxNumberOfRows, maxNumberOfColumns; + + private final YoInteger numberOfRows, numberOfColumns; + private final YoDouble[][] variables; + + public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, YoVariableRegistry registry) + { + this.maxNumberOfRows = maxNumberOfRows; + this.maxNumberOfColumns = maxNumberOfColumns; + + this.numberOfRows = new YoInteger(name + "NumRows", registry); + this.numberOfColumns = new YoInteger(name + "NumCols", registry); + + this.numberOfRows.set(maxNumberOfRows); + this.numberOfColumns.set(maxNumberOfColumns); + + variables = new YoDouble[maxNumberOfRows][maxNumberOfColumns]; + + for (int row = 0; row < maxNumberOfRows; row++) + { + for (int column = 0; column < maxNumberOfColumns; column++) + { + variables[row][column] = new YoDouble(name + "_" + row + "_" + column, registry); + } + } + } + + public void set(DenseMatrix64F matrix) + { + int numRows = matrix.getNumRows(); + int numCols = matrix.getNumCols(); + + if (((numRows > maxNumberOfRows) || (numCols > maxNumberOfColumns)) && (numRows > 0) && (numCols > 0)) + throw new RuntimeException("Not enough rows or columns. matrix to set is " + matrix.getNumRows() + " by " + matrix.getNumCols()); + + this.numberOfRows.set(numRows); + this.numberOfColumns.set(numCols); + + for (int row = 0; row < maxNumberOfRows; row++) + { + for (int column = 0; column < maxNumberOfColumns; column++) + { + if ((row < numRows) && (column < numCols)) + { + variables[row][column].set(matrix.get(row, column)); + } + else + { + variables[row][column].set(Double.NaN); + } + } + } + } + + public void getAndReshape(DenseMatrix64F matrixToPack) + { + matrixToPack.reshape(getNumberOfRows(), getNumberOfColumns()); + get(matrixToPack); + } + + public void get(DenseMatrix64F matrixToPack) + { + int numRows = matrixToPack.getNumRows(); + int numCols = matrixToPack.getNumCols(); + + if (((numRows > maxNumberOfRows) || (numCols > maxNumberOfColumns)) && (numRows > 0) && (numCols > 0)) + throw new RuntimeException("Not enough rows or columns. matrixToPack is " + matrixToPack.getNumRows() + " by " + matrixToPack.getNumCols()); + if ((numRows != this.numberOfRows.getIntegerValue()) || (numCols != this.numberOfColumns.getIntegerValue())) + throw new RuntimeException("Numer of rows and columns must be the same. Call getAndReshape() if you want to reshape the matrixToPack"); + + for (int row = 0; row < numRows; row++) + { + for (int column = 0; column < numCols; column++) + { + matrixToPack.set(row, column, variables[row][column].getDoubleValue()); + } + } + } + + public int getNumberOfRows() + { + return numberOfRows.getIntegerValue(); + } + + public int getNumberOfColumns() + { + return numberOfColumns.getIntegerValue(); + } + + public void setToZero(int numberOfRows, int numberOfColumns) + { + if (((numberOfRows > maxNumberOfRows) || (numberOfColumns > maxNumberOfColumns)) && (numberOfRows > 0) && (numberOfColumns > 0)) + throw new RuntimeException("Not enough rows or columns: " + numberOfRows + " by " + numberOfColumns); + + this.numberOfRows.set(numberOfRows); + this.numberOfColumns.set(numberOfColumns); + + for (int row = 0; row < maxNumberOfRows; row++) + { + for (int column = 0; column < maxNumberOfColumns; column++) + { + if ((row < numberOfRows) && (column < numberOfColumns)) + { + variables[row][column].set(0.0); + } + else + { + variables[row][column].set(Double.NaN); + } + } + } + + } +} From 2a11e119a2c9a34883e69f9d47300b674f37f859 Mon Sep 17 00:00:00 2001 From: Robert Date: Mon, 29 Jan 2018 17:23:05 -0600 Subject: [PATCH 02/47] made a rate limited yo frame pose made two methods use read only --- .../math/filters/RateLimitedYoFramePose.java | 94 +++++++++++++++++++ .../filters/RateLimitedYoFrameVector.java | 9 +- 2 files changed, 96 insertions(+), 7 deletions(-) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java new file mode 100644 index 00000000..84c49873 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java @@ -0,0 +1,94 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.robotics.math.frames.YoFramePose; +import us.ihmc.robotics.math.frames.YoFramePoseUsingQuaternions; +import us.ihmc.robotics.math.frames.YoFrameVector; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class RateLimitedYoFramePose extends YoFramePose +{ + private final RateLimitedYoFrameVector position; + private final RateLimitedYoFrameOrientation orientation; + + public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, + YoFramePose rawPose) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawPose, rawPose.getReferenceFrame()); + } + + public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); + } + + public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, YoFramePose rawPose) + { + this(namePrefix, nameSuffix, registry, null, dt, rawPose, rawPose.getReferenceFrame()); + setMaxRate(maxRate); + } + + public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, null, dt, null, referenceFrame); + setMaxRate(maxRate); + } + + private RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, + YoFramePose rawPose, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + if (rawPose != null) + { + this.position = new RateLimitedYoFrameVector(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, rawPose.getPosition()); + this.orientation = new RateLimitedYoFrameOrientation(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, rawPose.getOrientation()); + } + else + { + this.position = new RateLimitedYoFrameVector(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, referenceFrame); + this.orientation = new RateLimitedYoFrameOrientation(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, referenceFrame); + } + + + reset(); + } + + public void setMaxRate(double maxRate) + { + this.position.setMaxRate(maxRate); + this.orientation.setMaxRate(maxRate); + } + + public void reset() + { + position.reset(); + orientation.reset(); + } + + public void update() + { + position.update(); + orientation.update(); + + setPosition(position); + setOrientation(orientation.getFrameOrientation()); + } + + public void update(FramePose3DReadOnly framePoseUnfiltered) + { + checkReferenceFrameMatch(framePoseUnfiltered); + position.update(framePoseUnfiltered.getPosition()); + orientation.update(framePoseUnfiltered.getOrientation()); + + setPosition(position); + setOrientation(orientation.getFrameOrientation()); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java index 286207ad..ca4a50cc 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java @@ -3,6 +3,7 @@ import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.robotics.math.frames.YoFrameTuple; @@ -87,13 +88,7 @@ public void update() update(rawPosition); } - public void update(YoFrameTuple yoFrameVectorUnfiltered) - { - checkReferenceFrameMatch(yoFrameVectorUnfiltered); - update(yoFrameVectorUnfiltered.getX(), yoFrameVectorUnfiltered.getY(), yoFrameVectorUnfiltered.getZ()); - } - - public void update(FrameTuple3DBasics frameVectorUnfiltered) + public void update(FrameTuple3DReadOnly frameVectorUnfiltered) { checkReferenceFrameMatch(frameVectorUnfiltered); update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); From 9aa21eb5925157dc5915d40238c49d3a05699913 Mon Sep 17 00:00:00 2001 From: Jesper Smith Date: Wed, 31 Jan 2018 13:42:43 +0100 Subject: [PATCH 03/47] Added setAlpha to AlphaFilteredYoFrameVector --- .../robotics/math/filters/AlphaFilteredYoFrameVector.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java index 32bca05c..77f44471 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java @@ -124,4 +124,11 @@ public void reset() y.reset(); z.reset(); } + + public void setAlpha(double alpha) + { + x.setAlpha(alpha); + y.setAlpha(alpha); + z.setAlpha(alpha); + } } From 5cad081a174767b9b5110861e84916f132c124d7 Mon Sep 17 00:00:00 2001 From: Jesper Smith Date: Wed, 31 Jan 2018 15:05:37 +0100 Subject: [PATCH 04/47] Added support for DoubleProviders to alpha filtered variables. This allows the use of either parameters or yovariables. Also created a AlphaBasedOnBreakFrequencyProvider --- .../AlphaBasedOnBreakFrequencyProvider.java | 49 +++++++++++++++++++ .../AlphaFilteredWrappingYoVariable.java | 7 +-- .../filters/AlphaFilteredYoFramePoint.java | 9 ++-- .../filters/AlphaFilteredYoFramePoint2d.java | 10 ++-- .../AlphaFilteredYoFrameQuaternion.java | 26 +++++++--- .../filters/AlphaFilteredYoFrameVector.java | 17 ++----- .../filters/AlphaFilteredYoFrameVector2d.java | 12 ++--- .../math/filters/AlphaFilteredYoVariable.java | 36 +++++++++----- .../filters/RateLimitedYoFrameVector.java | 4 +- 9 files changed, 120 insertions(+), 50 deletions(-) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java new file mode 100644 index 00000000..cadb7148 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java @@ -0,0 +1,49 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.providers.DoubleProvider; + +/** + * This class calculates the alpha value given a break frequency provider + * + * The value gets cached to avoid unneccessary calculations + * + * @author jesper + * + */ +public class AlphaBasedOnBreakFrequencyProvider implements DoubleProvider +{ + + private final DoubleProvider breakFrequencyProvider; + private final double dt; + + private double previousBreakFrequency = Double.NaN; + private double alpha = 0.0; + + public AlphaBasedOnBreakFrequencyProvider(DoubleProvider breakFrequencyProvider, double dt) + { + this.breakFrequencyProvider = breakFrequencyProvider; + this.dt = dt; + } + + + @Override + /** + * 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. + * + */ + public double getValue() + { + double currentBreakFrequency = breakFrequencyProvider.getValue(); + if(currentBreakFrequency != previousBreakFrequency) + { + alpha = AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(currentBreakFrequency, dt); + previousBreakFrequency = currentBreakFrequency; + } + + return alpha; + } + +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java index 9ab23e07..4617a1d3 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java @@ -1,6 +1,7 @@ package us.ihmc.robotics.math.filters; import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoDouble; @@ -11,7 +12,7 @@ public class AlphaFilteredWrappingYoVariable extends AlphaFilteredYoVariable private double previousUnfilteredVariable; private final YoDouble unfilteredVariable; private final YoDouble unfilteredInRangeVariable; - private final YoDouble alphaVariable; + private final DoubleProvider alphaVariable; private final YoDouble temporaryOutputVariable; private final YoDouble error; @@ -21,7 +22,7 @@ public class AlphaFilteredWrappingYoVariable extends AlphaFilteredYoVariable //wrap the values in [lowerLimit ; upperLimit[ - public AlphaFilteredWrappingYoVariable(String name, String description, YoVariableRegistry registry, final YoDouble unfilteredVariable, YoDouble alphaVariable, double lowerLimit, double upperLimit) + public AlphaFilteredWrappingYoVariable(String name, String description, YoVariableRegistry registry, final YoDouble unfilteredVariable, DoubleProvider alphaVariable, double lowerLimit, double upperLimit) { super(name, description, registry, alphaVariable); this.alphaVariable = alphaVariable; @@ -96,7 +97,7 @@ public void update(double currentPosition) } } - temporaryOutputVariable.set(alphaVariable.getDoubleValue() * temporaryOutputVariable.getDoubleValue() + (1.0 - alphaVariable.getDoubleValue()) + temporaryOutputVariable.set(alphaVariable.getValue() * temporaryOutputVariable.getDoubleValue() + (1.0 - alphaVariable.getValue()) * unfilteredInRangeVariable.getDoubleValue()); if (temporaryOutputVariable.getDoubleValue() > upperLimit + EPSILON) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java index 11441ca3..0a774425 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java @@ -3,10 +3,11 @@ import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.robotics.math.frames.YoFramePoint; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; public class AlphaFilteredYoFramePoint extends YoFramePoint { @@ -63,7 +64,7 @@ public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String n public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, - YoDouble alpha, YoFramePoint unfilteredPoint) + DoubleProvider alpha, YoFramePoint unfilteredPoint) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); @@ -89,7 +90,7 @@ public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String n } public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, - YoDouble alphaX, YoDouble alphaY, YoDouble alphaZ, YoFramePoint unfilteredPoint) + DoubleProvider alphaX, DoubleProvider alphaY, DoubleProvider alphaZ, YoFramePoint unfilteredPoint) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alphaX, unfilteredPoint.getYoX()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java index 1c58601c..24b6387d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java @@ -3,10 +3,10 @@ import us.ihmc.euclid.referenceFrame.FramePoint2D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple2D.Point2D; -import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.robotics.math.frames.YoFramePoint2d; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; public class AlphaFilteredYoFramePoint2d extends YoFramePoint2d { @@ -31,12 +31,12 @@ public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(Stri return ret; } - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble alpha, ReferenceFrame referenceFrame) + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { return createAlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); } - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, YoDouble alpha, ReferenceFrame referenceFrame) + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, alpha); @@ -58,7 +58,7 @@ public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(Stri return ret; } - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble alpha, YoFramePoint2d unfilteredPoint) + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, YoFramePoint2d unfilteredPoint) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java index 5568a4aa..41a07efb 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java @@ -2,6 +2,7 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -10,7 +11,8 @@ public class AlphaFilteredYoFrameQuaternion extends YoFrameQuaternion implements ProcessingYoVariable { private final YoFrameQuaternion unfilteredQuaternion; - private final YoDouble alpha; + private final YoDouble yoAlpha; + private final DoubleProvider alpha; private final YoBoolean hasBeenCalled; private final Quaternion qMeasured = new Quaternion(); private final Quaternion qPreviousFiltered = new Quaternion(); @@ -29,18 +31,26 @@ public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoDo this(namePrefix, nameSuffix, null, alpha, referenceFrame, registry); } - public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, YoDouble alpha, + public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, DoubleProvider alpha, YoVariableRegistry registry) { this(namePrefix, nameSuffix, unfilteredQuaternion, alpha, unfilteredQuaternion.getReferenceFrame(), registry); } - private AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, YoDouble alpha, + private AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, DoubleProvider alpha, ReferenceFrame referenceFrame, YoVariableRegistry registry) { super(namePrefix, nameSuffix, referenceFrame, registry); this.unfilteredQuaternion = unfilteredQuaternion; this.alpha = alpha; + if(alpha instanceof YoDouble) + { + yoAlpha = (YoDouble) alpha; + } + else + { + yoAlpha = null; + } this.hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); } @@ -63,7 +73,7 @@ public void update(Quaternion qMeasured) { qPreviousFiltered.set(this); - qNewFiltered.interpolate(qMeasured, qPreviousFiltered, alpha.getDoubleValue()); // qPreviousFiltered 'gets multiplied by alpha' + qNewFiltered.interpolate(qMeasured, qPreviousFiltered, alpha.getValue()); // qPreviousFiltered 'gets multiplied by alpha' set(qNewFiltered); } else @@ -75,12 +85,16 @@ public void update(Quaternion qMeasured) public void setAlpha(double alpha) { - this.alpha.set(alpha); + if(yoAlpha == null) + { + throw new RuntimeException("This AlphaFilteredYoFrameVector is not backed by a yoVariable"); + } + this.yoAlpha.set(alpha); } public void setBreakFrequency(double breakFrequencyHertz, double dt) { - this.alpha.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequencyHertz, dt)); + setAlpha(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequencyHertz, dt)); } public void reset() diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java index 77f44471..af8f60a9 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java @@ -3,11 +3,11 @@ import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.robotics.math.frames.YoFramePoint; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; import us.ihmc.robotics.math.frames.YoFrameVector; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; public class AlphaFilteredYoFrameVector extends YoFrameVector implements ProcessingYoVariable { @@ -36,7 +36,7 @@ public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String } public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, - YoDouble alpha, ReferenceFrame referenceFrame) + DoubleProvider alpha, ReferenceFrame referenceFrame) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); @@ -64,7 +64,7 @@ public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, - YoDouble alpha, YoFrameVector unfilteredVector) + DoubleProvider alpha, YoFrameVector unfilteredVector) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); @@ -77,7 +77,7 @@ public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String } public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, - YoDouble alpha, YoFramePoint unfilteredPosition) + DoubleProvider alpha, YoFramePoint unfilteredPosition) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPosition.getYoX()); @@ -124,11 +124,4 @@ public void reset() y.reset(); z.reset(); } - - public void setAlpha(double alpha) - { - x.setAlpha(alpha); - y.setAlpha(alpha); - z.setAlpha(alpha); - } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java index ac4a535f..def769fc 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java @@ -3,10 +3,10 @@ import us.ihmc.euclid.referenceFrame.FrameVector2D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple2D.Vector2D; -import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; import us.ihmc.robotics.math.frames.YoFrameVector2d; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; public class AlphaFilteredYoFrameVector2d extends YoFrameVector2d { @@ -31,12 +31,12 @@ public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(St return ret; } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble alpha, ReferenceFrame referenceFrame) + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, YoDouble alpha, ReferenceFrame referenceFrame) + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, alpha); AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, registry, alpha); @@ -58,12 +58,12 @@ public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(St } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble alpha, YoFrameVector2d unfilteredVector) + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, YoFrameVector2d unfilteredVector) { return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, unfilteredVector); } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, YoDouble alpha, YoFrameVector2d unfilteredVector) + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, DoubleProvider alpha, YoFrameVector2d unfilteredVector) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, alpha, unfilteredVector.getYoX()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java index ab994d3c..20c41b57 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java @@ -1,6 +1,7 @@ package us.ihmc.robotics.math.filters; import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -29,7 +30,8 @@ */ public class AlphaFilteredYoVariable extends YoDouble implements ProcessingYoVariable { - private final YoDouble alphaVariable; + private final YoDouble yoAlphaVariable; + private final DoubleProvider alphaVariable; private final YoDouble position; protected final YoBoolean hasBeenCalled; @@ -43,39 +45,47 @@ public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, double { super(name,registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - this.alphaVariable = new YoDouble(name + "AlphaVariable", registry); - this.alphaVariable.set(alpha); + this.yoAlphaVariable = new YoDouble(name + "AlphaVariable", registry); + this.yoAlphaVariable.set(alpha); + this.alphaVariable = yoAlphaVariable; this.position = positionVariable; reset(); } - public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, YoDouble alphaVariable) + public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, DoubleProvider alphaVariable) { this(name, "", registry, alphaVariable, null); } - public AlphaFilteredYoVariable(String name, String description, YoVariableRegistry registry, YoDouble alphaVariable) + public AlphaFilteredYoVariable(String name, String description, YoVariableRegistry registry, DoubleProvider alphaVariable) { this(name, description, registry, alphaVariable, null); } - public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, YoDouble alphaVariable, YoDouble positionVariable) + public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, DoubleProvider alphaVariable, YoDouble positionVariable) { this(name, "", registry, alphaVariable, positionVariable); } - public AlphaFilteredYoVariable(String name, String description, YoVariableRegistry registry, YoDouble alphaVariable, YoDouble positionVariable) + 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; this.alphaVariable = alphaVariable; - + if(alphaVariable instanceof YoDouble) + { + this.yoAlphaVariable = (YoDouble) alphaVariable; + } + else + { + this.yoAlphaVariable = null; + } + reset(); } - public void reset() { hasBeenCalled.set(false); @@ -101,13 +111,17 @@ public void update(double currentPosition) } - set(alphaVariable.getDoubleValue() * getDoubleValue() + (1.0 - alphaVariable.getDoubleValue()) * currentPosition); + set(alphaVariable.getValue() * getDoubleValue() + (1.0 - alphaVariable.getValue()) * currentPosition); } public void setAlpha(double alpha) { - this.alphaVariable.set(alpha); + if(this.yoAlphaVariable == null) + { + throw new RuntimeException("Cannot set alpha, this filter is not backed by a yoVariable"); + } + this.yoAlphaVariable.set(alpha); } /** diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java index ca4a50cc..e57a7205 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java @@ -2,15 +2,13 @@ import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DBasics; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.robotics.math.frames.YoFrameTuple; +import us.ihmc.robotics.math.frames.YoFrameVector; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.robotics.math.frames.YoFrameVector; public class RateLimitedYoFrameVector extends YoFrameVector { From fa17bb7e3162a7a9193ab47a22df97851c6a6229 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Wed, 31 Jan 2018 18:03:28 -0600 Subject: [PATCH 05/47] Created a rate limited YoFrameQuaternion. --- .../RateLimitedYoFrameOrientation.java | 105 ++++++++----- .../filters/RateLimitedYoFrameQuaternion.java | 141 ++++++++++++++++++ 2 files changed, 208 insertions(+), 38 deletions(-) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java index 190ff7de..c0c6ac40 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java @@ -1,61 +1,69 @@ package us.ihmc.robotics.math.filters; import us.ihmc.euclid.referenceFrame.FrameQuaternion; -import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.robotics.math.frames.YoFrameOrientation; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.robotics.math.frames.YoFrameOrientation; public class RateLimitedYoFrameOrientation extends YoFrameOrientation { - private final YoDouble maxRateVariable; + private final DoubleProvider maxRateVariable; private final YoFrameOrientation rawOrientation; private final YoBoolean limited; private final YoBoolean hasBeenCalled; private final double dt; - private final FrameVector3D differenceVector = new FrameVector3D(); + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, - YoFrameOrientation rawOrientation) + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + YoFrameOrientation rawOrientation) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawOrientation, rawOrientation.getReferenceFrame()); } - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, - ReferenceFrame referenceFrame) + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); } - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, YoFrameOrientation rawOrientation) + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + YoFrameOrientation rawOrientation) { - this(namePrefix, nameSuffix, registry, null, dt, rawOrientation, rawOrientation.getReferenceFrame()); - setMaxRate(maxRate); + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawOrientation, + rawOrientation.getReferenceFrame()); } - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + ReferenceFrame referenceFrame) { - this(namePrefix, nameSuffix, registry, null, dt, null, referenceFrame); - setMaxRate(maxRate); + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); } - private RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, - YoFrameOrientation rawOrientation, ReferenceFrame referenceFrame) + private RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + YoFrameOrientation rawOrientation, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); - if (maxRate != null) - this.maxRateVariable = maxRate; - else - this.maxRateVariable = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + if (maxRate == null) + maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; this.rawOrientation = rawOrientation; @@ -64,11 +72,6 @@ private RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVa reset(); } - public void setMaxRate(double maxRate) - { - this.maxRateVariable.set(maxRate); - } - public void reset() { hasBeenCalled.set(false); @@ -89,36 +92,62 @@ public void update(YoFrameOrientation yoFrameVectorUnfiltered) { checkReferenceFrameMatch(yoFrameVectorUnfiltered); update(yoFrameVectorUnfiltered.getYaw().getDoubleValue(), yoFrameVectorUnfiltered.getPitch().getDoubleValue(), - yoFrameVectorUnfiltered.getRoll().getDoubleValue()); + yoFrameVectorUnfiltered.getRoll().getDoubleValue()); } public void update(FrameQuaternion frameOrientationUnfiltered) { checkReferenceFrameMatch(frameOrientationUnfiltered); - update(frameOrientationUnfiltered); + update((QuaternionReadOnly) frameOrientationUnfiltered); } - public void update(QuaternionReadOnly quaternionUnfiltered) + private final Quaternion quaternionUnfiltered = new Quaternion(); + + public void update(double yawUnfiltered, double pitchUnfiltered, double rollUnfiltered) { - update(quaternionUnfiltered.getYaw(), quaternionUnfiltered.getPitch(), quaternionUnfiltered.getRoll()); + quaternionUnfiltered.setYawPitchRoll(yawUnfiltered, pitchUnfiltered, rollUnfiltered); + update(quaternionUnfiltered); } - public void update(double yawUnfiltered, double pitchUnfiltered, double rollUnfiltered) + private final Quaternion quaternionFiltered = new Quaternion(); + private final Quaternion difference = new Quaternion(); + private final Vector3D limitedRotationVector = new Vector3D(); + + public void update(QuaternionReadOnly quaternionUnfiltered) { if (!hasBeenCalled.getBooleanValue() || containsNaN()) { hasBeenCalled.set(true); - setYawPitchRoll(yawUnfiltered, pitchUnfiltered, rollUnfiltered); + limited.set(false); + set(quaternionUnfiltered); + return; } - if (maxRateVariable.getDoubleValue() < 0) - throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); + quaternionFiltered.set(getFrameOrientation()); - differenceVector.setToZero(getReferenceFrame()); - differenceVector.set(yawUnfiltered, pitchUnfiltered, rollUnfiltered); - differenceVector.sub(getYaw().getDoubleValue(), getPitch().getDoubleValue(), getRoll().getDoubleValue()); + if (quaternionFiltered.dot(quaternionUnfiltered) > 0.0) + { + difference.difference(quaternionFiltered, quaternionUnfiltered); + } + else + { + difference.setAndNegate(quaternionUnfiltered); + difference.preMultiplyConjugateOther(quaternionFiltered); + } - limited.set(differenceVector.clipToMaxLength(maxRateVariable.getDoubleValue() * dt)); - add(differenceVector.getX(), differenceVector.getY(), differenceVector.getZ()); + difference.get(limitedRotationVector); + boolean clipped = limitedRotationVector.clipToMaxLength(dt * maxRateVariable.getValue()); + limited.set(clipped); + + if (clipped) + { + difference.set(limitedRotationVector); + quaternionFiltered.multiply(difference); + set(quaternionFiltered); + } + else + { + set(quaternionUnfiltered); + } } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java new file mode 100644 index 00000000..7700bd1b --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java @@ -0,0 +1,141 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FrameQuaternion; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.robotics.math.frames.YoFrameQuaternion; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class RateLimitedYoFrameQuaternion extends YoFrameQuaternion +{ + private final QuaternionReadOnly rawQuaternion; + private final YoBoolean hasBeenCalled; + private final double dt; + + private final YoBoolean limited; + private final DoubleProvider maxRateVariable; + + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + FrameQuaternionReadOnly rawQuaternion) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawQuaternion.getReferenceFrame(), + rawQuaternion); + } + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, referenceFrame, null); + } + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + ReferenceFrame referenceFrame, QuaternionReadOnly rawQuaternion) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, referenceFrame, rawQuaternion); + } + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + FrameQuaternionReadOnly rawQuaternion) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawQuaternion.getReferenceFrame(), rawQuaternion); + } + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, referenceFrame, null); + } + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame, QuaternionReadOnly rawQuaternion) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); + limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); + + if (maxRate == null) + maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; + + this.rawQuaternion = rawQuaternion; + + this.dt = dt; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawQuaternion == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawQuaternion); + } + + public void update(FrameQuaternion frameOrientationUnfiltered) + { + checkReferenceFrameMatch(frameOrientationUnfiltered); + update(frameOrientationUnfiltered); + } + + private final Quaternion difference = new Quaternion(); + private final Vector3D limitedRotationVector = new Vector3D(); + + public void update(QuaternionReadOnly quaternionUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + limited.set(false); + set(quaternionUnfiltered); + return; + } + + if (dot(quaternionUnfiltered) > 0.0) + { + difference.difference(this, quaternionUnfiltered); + } + else + { + difference.setAndNegate(quaternionUnfiltered); + difference.preMultiplyConjugateOther(this); + } + + difference.get(limitedRotationVector); + boolean clipped = limitedRotationVector.clipToMaxLength(dt * maxRateVariable.getValue()); + limited.set(clipped); + + if (clipped) + { + difference.set(limitedRotationVector); + multiply(difference); + } + else + { + set(quaternionUnfiltered); + } + } +} From 203bfedafa2d106c66ce59edd12309d39837904c Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Wed, 31 Jan 2018 18:29:11 -0600 Subject: [PATCH 06/47] Cleaned up some the rate limited yo-variables. Also changed RateLimitedYoFramePose to use a YoFrameQuaternion instead of a YoFrameOrientation. Changed RateLimitedYoFramePose to create less YoVariables. I'm not really happy about having to duplicate RateLimitedYoFrameVector to create RateLimitedYoFramePoint. That should be easy to cleanup later though. Created AlphaBasedOnBreakFrequencyProvider. Changed the alpha filter to be compatible with parameter. Propagated the change to the classes using the AlphaFilteredYoVariable. Made the same change to the alpha filtered yo frame quaternion. Forgot to create alpha when not provided. Updated codebase. --- .../AlphaBasedOnBreakFrequencyProvider.java | 13 +- .../filters/AlphaFilteredYoFramePoint.java | 9 +- .../AlphaFilteredYoFrameQuaternion.java | 41 ++---- .../math/filters/AlphaFilteredYoVariable.java | 40 +++--- .../RateLimitedYoFrameOrientation.java | 4 +- .../math/filters/RateLimitedYoFramePoint.java | 120 ++++++++++++++++++ .../math/filters/RateLimitedYoFramePose.java | 90 +++++++------ .../filters/RateLimitedYoFrameQuaternion.java | 4 +- .../filters/RateLimitedYoFrameVector.java | 50 ++++---- 9 files changed, 245 insertions(+), 126 deletions(-) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java index cadb7148..a0850252 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java @@ -12,37 +12,36 @@ */ public class AlphaBasedOnBreakFrequencyProvider implements DoubleProvider { - + private final DoubleProvider breakFrequencyProvider; private final double dt; - + private double previousBreakFrequency = Double.NaN; private double alpha = 0.0; - + public AlphaBasedOnBreakFrequencyProvider(DoubleProvider breakFrequencyProvider, double dt) { this.breakFrequencyProvider = breakFrequencyProvider; this.dt = dt; } - @Override /** * 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. + * The value gets cached, and checked based on the current value of the breakFrequencyProvider. * This will be safe to rewind. * */ public double getValue() { double currentBreakFrequency = breakFrequencyProvider.getValue(); - if(currentBreakFrequency != previousBreakFrequency) + if (currentBreakFrequency != previousBreakFrequency) { alpha = AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(currentBreakFrequency, dt); previousBreakFrequency = currentBreakFrequency; } - + return alpha; } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java index 0a774425..eff82a24 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java @@ -7,7 +7,6 @@ import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoDouble; public class AlphaFilteredYoFramePoint extends YoFramePoint { @@ -36,7 +35,7 @@ public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String n } public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, - YoDouble alpha, ReferenceFrame referenceFrame) + DoubleProvider alpha, ReferenceFrame referenceFrame) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); @@ -64,7 +63,11 @@ public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String n public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, +<<<<<<< HEAD DoubleProvider alpha, YoFramePoint unfilteredPoint) +======= + DoubleProvider alpha, YoFramePoint unfilteredPoint) +>>>>>>> a7cecfc (Propagated the change to the classes using the AlphaFilteredYoVariable.) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); @@ -90,7 +93,7 @@ public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String n } public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, - DoubleProvider alphaX, DoubleProvider alphaY, DoubleProvider alphaZ, YoFramePoint unfilteredPoint) + DoubleProvider alphaX, DoubleProvider alphaY, DoubleProvider alphaZ, YoFramePoint unfilteredPoint) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alphaX, unfilteredPoint.getYoX()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java index 41a07efb..14de6f1d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java @@ -2,30 +2,35 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.robotics.math.frames.YoFrameQuaternion; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.robotics.math.frames.YoFrameQuaternion; public class AlphaFilteredYoFrameQuaternion extends YoFrameQuaternion implements ProcessingYoVariable { private final YoFrameQuaternion unfilteredQuaternion; - private final YoDouble yoAlpha; private final DoubleProvider alpha; private final YoBoolean hasBeenCalled; private final Quaternion qMeasured = new Quaternion(); private final Quaternion qPreviousFiltered = new Quaternion(); private final Quaternion qNewFiltered = new Quaternion(); + private static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoVariableRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "AlphaVariable", registry); + maxRate.set(initialValue); + return maxRate; + } + public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, double alpha, YoVariableRegistry registry) { - this(namePrefix, nameSuffix, unfilteredQuaternion, new YoDouble(namePrefix + "Alpha", registry), registry); - this.setAlpha(alpha); + this(namePrefix, nameSuffix, unfilteredQuaternion, createAlphaYoDouble(namePrefix, alpha, registry), registry); } - public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoDouble alpha, ReferenceFrame referenceFrame, + public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, DoubleProvider alpha, ReferenceFrame referenceFrame, YoVariableRegistry registry) { this(namePrefix, nameSuffix, null, alpha, referenceFrame, registry); @@ -42,15 +47,11 @@ private AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoF { super(namePrefix, nameSuffix, referenceFrame, registry); this.unfilteredQuaternion = unfilteredQuaternion; + + if (alpha == null) + alpha = createAlphaYoDouble(namePrefix, 0.0, registry); this.alpha = alpha; - if(alpha instanceof YoDouble) - { - yoAlpha = (YoDouble) alpha; - } - else - { - yoAlpha = null; - } + this.hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); } @@ -83,20 +84,6 @@ public void update(Quaternion qMeasured) } } - public void setAlpha(double alpha) - { - if(yoAlpha == null) - { - throw new RuntimeException("This AlphaFilteredYoFrameVector is not backed by a yoVariable"); - } - this.yoAlpha.set(alpha); - } - - public void setBreakFrequency(double breakFrequencyHertz, double dt) - { - setAlpha(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(breakFrequencyHertz, dt)); - } - public void reset() { hasBeenCalled.set(false); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java index 20c41b57..0d59f6a5 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java @@ -30,12 +30,18 @@ */ public class AlphaFilteredYoVariable extends YoDouble implements ProcessingYoVariable { - private final YoDouble yoAlphaVariable; private final DoubleProvider alphaVariable; private final YoDouble position; protected final YoBoolean hasBeenCalled; + private static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoVariableRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "AlphaVariable", registry); + maxRate.set(initialValue); + return maxRate; + } + public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, double alpha) { this(name, registry, alpha, null); @@ -43,13 +49,7 @@ public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, double public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, double alpha, YoDouble positionVariable) { - super(name,registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - this.yoAlphaVariable = new YoDouble(name + "AlphaVariable", registry); - this.yoAlphaVariable.set(alpha); - this.alphaVariable = yoAlphaVariable; - this.position = positionVariable; - reset(); + this(name, "", registry, createAlphaYoDouble(name, alpha, registry), positionVariable); } public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, DoubleProvider alphaVariable) @@ -62,7 +62,6 @@ public AlphaFilteredYoVariable(String name, String description, YoVariableRegist this(name, description, registry, alphaVariable, null); } - public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, DoubleProvider alphaVariable, YoDouble positionVariable) { this(name, "", registry, alphaVariable, positionVariable); @@ -73,6 +72,9 @@ public AlphaFilteredYoVariable(String name, String description, YoVariableRegist 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; if(alphaVariable instanceof YoDouble) { @@ -110,18 +112,8 @@ public void update(double currentPosition) set(currentPosition); } - - set(alphaVariable.getValue() * getDoubleValue() + (1.0 - alphaVariable.getValue()) * currentPosition); - - } - - public void setAlpha(double alpha) - { - if(this.yoAlphaVariable == null) - { - throw new RuntimeException("Cannot set alpha, this filter is not backed by a yoVariable"); - } - this.yoAlphaVariable.set(alpha); + double alpha = alphaVariable.getValue(); + set(alpha * getDoubleValue() + (1.0 - alpha) * currentPosition); } /** @@ -165,7 +157,7 @@ public static void main(String[] args) { double dt = 1 / 1e3; - for (double i = 2; i < 1.0/dt; i = i * 1.2) + for (double i = 2; i < 1.0 / dt; i = i * 1.2) { double alpha = computeAlphaGivenBreakFrequency(i, dt); double alphaProperly = computeAlphaGivenBreakFrequencyProperly(i, dt); @@ -176,10 +168,10 @@ public static void main(String[] args) System.out.println(computeAlphaGivenBreakFrequencyProperly(20, 0.006)); System.out.println(computeAlphaGivenBreakFrequencyProperly(20, 0.003)); } - + public boolean getHasBeenCalled() { - return hasBeenCalled.getBooleanValue(); + return hasBeenCalled.getBooleanValue(); } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java index c0c6ac40..d3d09342 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java @@ -135,13 +135,13 @@ public void update(QuaternionReadOnly quaternionUnfiltered) difference.preMultiplyConjugateOther(quaternionFiltered); } - difference.get(limitedRotationVector); + difference.getRotationVector(limitedRotationVector); boolean clipped = limitedRotationVector.clipToMaxLength(dt * maxRateVariable.getValue()); limited.set(clipped); if (clipped) { - difference.set(limitedRotationVector); + difference.setRotationVector(limitedRotationVector); quaternionFiltered.multiply(difference); set(quaternionFiltered); } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java new file mode 100644 index 00000000..089cc9f5 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java @@ -0,0 +1,120 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.robotics.math.frames.YoFramePoint; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class RateLimitedYoFramePoint extends YoFramePoint +{ + private final DoubleProvider maxRateVariable; + + private final FrameTuple3DReadOnly rawPosition; + private final YoBoolean limited; + private final YoBoolean hasBeenCalled; + private final double dt; + + private final FrameVector3D differenceVector = new FrameVector3D(); + + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } + + public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); + } + + public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); + } + + public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + FrameTuple3DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, + rawPosition.getReferenceFrame()); + } + + public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); + } + + private RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); + this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); + + if (maxRate == null) + maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; + + this.rawPosition = rawPosition; + + this.dt = dt; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawPosition); + } + + public void update(FrameTuple3DReadOnly frameVectorUnfiltered) + { + checkReferenceFrameMatch(frameVectorUnfiltered); + update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); + } + + public void update(Tuple3DReadOnly vectorUnfiltered) + { + update(vectorUnfiltered.getX(), vectorUnfiltered.getY(), vectorUnfiltered.getZ()); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered, zUnfiltered); + } + + if (maxRateVariable.getValue() < 0) + throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); + + differenceVector.setToZero(getReferenceFrame()); + differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); + differenceVector.sub(getX(), getY(), getZ()); + + limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); + add(differenceVector); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java index 84c49873..e41da55a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java @@ -1,72 +1,66 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.euclid.referenceFrame.FramePose3D; -import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.geometry.tools.EuclidGeometryIOTools; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePose3DBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameQuaternionBasics; import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; -import us.ihmc.robotics.math.frames.YoFramePose; -import us.ihmc.robotics.math.frames.YoFramePoseUsingQuaternions; -import us.ihmc.robotics.math.frames.YoFrameVector; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -public class RateLimitedYoFramePose extends YoFramePose +public class RateLimitedYoFramePose implements FixedFramePose3DBasics { - private final RateLimitedYoFrameVector position; - private final RateLimitedYoFrameOrientation orientation; + private final RateLimitedYoFramePoint position; + private final RateLimitedYoFrameQuaternion orientation; - public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, - YoFramePose rawPose) + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } + + public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + FramePose3DReadOnly rawPose) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawPose, rawPose.getReferenceFrame()); } - public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, + public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); } - public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, YoFramePose rawPose) + public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, FramePose3DReadOnly rawPose) { - this(namePrefix, nameSuffix, registry, null, dt, rawPose, rawPose.getReferenceFrame()); - setMaxRate(maxRate); + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPose, rawPose.getReferenceFrame()); } public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { - this(namePrefix, nameSuffix, registry, null, dt, null, referenceFrame); - setMaxRate(maxRate); + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); } - private RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, - YoFramePose rawPose, ReferenceFrame referenceFrame) + private RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + FramePose3DReadOnly rawPose, ReferenceFrame referenceFrame) { - super(namePrefix, nameSuffix, referenceFrame, registry); - if (rawPose != null) { - this.position = new RateLimitedYoFrameVector(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, rawPose.getPosition()); - this.orientation = new RateLimitedYoFrameOrientation(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, rawPose.getOrientation()); + this.position = new RateLimitedYoFramePoint(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, rawPose.getPosition()); + this.orientation = new RateLimitedYoFrameQuaternion(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, rawPose.getOrientation()); } else { - this.position = new RateLimitedYoFrameVector(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, referenceFrame); - this.orientation = new RateLimitedYoFrameOrientation(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, referenceFrame); + this.position = new RateLimitedYoFramePoint(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, referenceFrame); + this.orientation = new RateLimitedYoFrameQuaternion(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, referenceFrame); } - reset(); } - public void setMaxRate(double maxRate) - { - this.position.setMaxRate(maxRate); - this.orientation.setMaxRate(maxRate); - } - public void reset() { position.reset(); @@ -78,8 +72,7 @@ public void update() position.update(); orientation.update(); - setPosition(position); - setOrientation(orientation.getFrameOrientation()); + set(position, orientation); } public void update(FramePose3DReadOnly framePoseUnfiltered) @@ -88,7 +81,30 @@ public void update(FramePose3DReadOnly framePoseUnfiltered) position.update(framePoseUnfiltered.getPosition()); orientation.update(framePoseUnfiltered.getOrientation()); - setPosition(position); - setOrientation(orientation.getFrameOrientation()); + set(position, orientation); + } + + @Override + public FixedFramePoint3DBasics getPosition() + { + return position; + } + + @Override + public FixedFrameQuaternionBasics getOrientation() + { + return orientation; + } + + @Override + public ReferenceFrame getReferenceFrame() + { + return position.getReferenceFrame(); + } + + @Override + public String toString() + { + return EuclidGeometryIOTools.getPose3DString(this) + "-" + getReferenceFrame(); } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java index 7700bd1b..4b45ec5c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java @@ -124,13 +124,13 @@ public void update(QuaternionReadOnly quaternionUnfiltered) difference.preMultiplyConjugateOther(this); } - difference.get(limitedRotationVector); + difference.getRotationVector(limitedRotationVector); boolean clipped = limitedRotationVector.clipToMaxLength(dt * maxRateVariable.getValue()); limited.set(clipped); if (clipped) { - difference.set(limitedRotationVector); + difference.setRotationVector(limitedRotationVector); multiply(difference); } else diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java index e57a7205..1492a13e 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java @@ -4,59 +4,66 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.robotics.math.frames.YoFrameTuple; import us.ihmc.robotics.math.frames.YoFrameVector; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; public class RateLimitedYoFrameVector extends YoFrameVector { - private final YoDouble maxRateVariable; + private final DoubleProvider maxRateVariable; - private final YoFrameTuple rawPosition; + private final FrameTuple3DReadOnly rawPosition; private final YoBoolean limited; private final YoBoolean hasBeenCalled; private final double dt; private final FrameVector3D differenceVector = new FrameVector3D(); - public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, - YoFrameTuple rawPosition) + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } + + public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); } - public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, + public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); } - public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, YoFrameTuple rawPosition) + public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + FrameTuple3DReadOnly rawPosition) { - this(namePrefix, nameSuffix, registry, null, dt, rawPosition, rawPosition.getReferenceFrame()); - setMaxRate(maxRate); + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, + rawPosition.getReferenceFrame()); } public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { - this(namePrefix, nameSuffix, registry, null, dt, null, referenceFrame); - setMaxRate(maxRate); + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); } - private RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, - YoFrameTuple rawPosition, ReferenceFrame referenceFrame) + private RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); - if (maxRate != null) - this.maxRateVariable = maxRate; - else - this.maxRateVariable = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + if (maxRate == null) + maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; this.rawPosition = rawPosition; @@ -65,11 +72,6 @@ private RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariabl reset(); } - public void setMaxRate(double maxRate) - { - this.maxRateVariable.set(maxRate); - } - public void reset() { hasBeenCalled.set(false); @@ -105,14 +107,14 @@ public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) set(xUnfiltered, yUnfiltered, zUnfiltered); } - if (maxRateVariable.getDoubleValue() < 0) + if (maxRateVariable.getValue() < 0) throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); differenceVector.setToZero(getReferenceFrame()); differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); differenceVector.sub(getX(), getY(), getZ()); - limited.set(differenceVector.clipToMaxLength(maxRateVariable.getDoubleValue() * dt)); + limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); add(differenceVector); } } From 87dc0c8ed05c71cad9386c7a2cf2fcb1698df10b Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Sun, 8 Apr 2018 16:58:50 -0500 Subject: [PATCH 07/47] Switched to use YoFrame objects from ihmc-yovariables. --- .../AccelerationLimitedYoFrameVector2d.java | 6 +++--- .../math/filters/AlphaFilteredYoFramePoint.java | 16 ++++++---------- .../filters/AlphaFilteredYoFramePoint2d.java | 8 ++++---- .../filters/AlphaFilteredYoFrameQuaternion.java | 2 +- .../math/filters/AlphaFilteredYoFrameVector.java | 12 ++++++------ .../filters/AlphaFilteredYoFrameVector2d.java | 10 +++++----- ...acklashCompensatingVelocityYoFrameVector.java | 14 +++++++------- .../math/filters/BetaFilteredYoFramePoint2d.java | 6 +++--- .../filters/BetaFilteredYoFrameVector2d.java | 6 +++--- .../math/filters/DeadzoneYoFrameVector.java | 10 +++++----- .../filters/FilteredVelocityYoFrameVector.java | 12 ++++++------ .../filters/FilteredVelocityYoFrameVector2d.java | 14 +++++++------- ...teDifferenceAngularVelocityYoFrameVector.java | 6 +++--- .../filters/RateLimitedYoFrameOrientation.java | 14 +++++++------- .../math/filters/RateLimitedYoFramePoint.java | 4 ++-- .../filters/RateLimitedYoFrameQuaternion.java | 2 +- .../math/filters/RateLimitedYoFrameVector.java | 4 ++-- .../math/filters/RateLimitedYoFrameVector2d.java | 8 ++++---- .../SecondOrderFilteredYoFrameVector.java | 8 ++++---- ...SimpleMovingAverageFilteredYoFrameVector.java | 6 +++--- 20 files changed, 82 insertions(+), 86 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java index c9a99b76..dc06b481 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java @@ -7,10 +7,10 @@ import us.ihmc.euclid.tuple2D.Vector2D; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFrameVector2D; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.robotics.math.frames.YoFrameVector2d; -public class AccelerationLimitedYoFrameVector2d extends YoFrameVector2d +public class AccelerationLimitedYoFrameVector2d extends YoFrameVector2D { private final AccelerationLimitedYoVariable x, y; @@ -35,7 +35,7 @@ public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFram public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, - YoDouble maxRate, YoDouble maxAcceleration, double dt, YoFrameVector2d unfilteredVector) + YoDouble maxRate, YoDouble maxAcceleration, double dt, YoFrameVector2D unfilteredVector) { AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoX(), dt); AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoY(), dt); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java index eff82a24..0495f951 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java @@ -3,12 +3,12 @@ import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.robotics.math.frames.YoFramePoint; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoFramePoint3D; -public class AlphaFilteredYoFramePoint extends YoFramePoint +public class AlphaFilteredYoFramePoint extends YoFramePoint3D { private final AlphaFilteredYoVariable x, y, z; @@ -49,7 +49,7 @@ public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String n public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, - YoFramePoint unfilteredPoint) + YoFramePoint3D unfilteredPoint) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); @@ -63,11 +63,7 @@ public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String n public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, -<<<<<<< HEAD - DoubleProvider alpha, YoFramePoint unfilteredPoint) -======= - DoubleProvider alpha, YoFramePoint unfilteredPoint) ->>>>>>> a7cecfc (Propagated the change to the classes using the AlphaFilteredYoVariable.) + DoubleProvider alpha, YoFramePoint3D unfilteredPoint) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); @@ -80,7 +76,7 @@ >>>>>>> a7cecfc (Propagated the change to the classes using the AlphaFilteredYoV } public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, - double alphaX, double alphaY, double alphaZ, YoFramePoint unfilteredPoint) + double alphaX, double alphaY, double alphaZ, YoFramePoint3D unfilteredPoint) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alphaX, unfilteredPoint.getYoX()); @@ -93,7 +89,7 @@ public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String n } public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, - DoubleProvider alphaX, DoubleProvider alphaY, DoubleProvider alphaZ, YoFramePoint unfilteredPoint) + DoubleProvider alphaX, DoubleProvider alphaY, DoubleProvider alphaZ, YoFramePoint3D unfilteredPoint) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alphaX, unfilteredPoint.getYoX()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java index 24b6387d..ba3506c0 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java @@ -3,12 +3,12 @@ import us.ihmc.euclid.referenceFrame.FramePoint2D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple2D.Point2D; -import us.ihmc.robotics.math.frames.YoFramePoint2d; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoFramePoint2D; -public class AlphaFilteredYoFramePoint2d extends YoFramePoint2d +public class AlphaFilteredYoFramePoint2d extends YoFramePoint2D { private final AlphaFilteredYoVariable x, y; @@ -47,7 +47,7 @@ public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(Stri return ret; } - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, YoFramePoint2d unfilteredPoint) + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, YoFramePoint2D unfilteredPoint) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); @@ -58,7 +58,7 @@ public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(Stri return ret; } - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, YoFramePoint2d unfilteredPoint) + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, YoFramePoint2D unfilteredPoint) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java index 14de6f1d..76d4defa 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java @@ -2,11 +2,11 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.robotics.math.frames.YoFrameQuaternion; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFrameQuaternion; public class AlphaFilteredYoFrameQuaternion extends YoFrameQuaternion implements ProcessingYoVariable { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java index af8f60a9..554c9e7c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java @@ -3,13 +3,13 @@ import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.robotics.math.frames.YoFramePoint; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.robotics.math.frames.YoFrameVector; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoFramePoint3D; +import us.ihmc.yoVariables.variable.YoFrameVector3D; -public class AlphaFilteredYoFrameVector extends YoFrameVector implements ProcessingYoVariable +public class AlphaFilteredYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable { private final AlphaFilteredYoVariable x, y, z; @@ -50,7 +50,7 @@ public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, - YoFrameVector unfilteredVector) + YoFrameVector3D unfilteredVector) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); @@ -64,7 +64,7 @@ public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, - DoubleProvider alpha, YoFrameVector unfilteredVector) + DoubleProvider alpha, YoFrameVector3D unfilteredVector) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); @@ -77,7 +77,7 @@ public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String } public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, - DoubleProvider alpha, YoFramePoint unfilteredPosition) + DoubleProvider alpha, YoFramePoint3D unfilteredPosition) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPosition.getYoX()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java index def769fc..06c9d8d5 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java @@ -4,11 +4,11 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple2D.Vector2D; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.robotics.math.frames.YoFrameVector2d; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoFrameVector2D; -public class AlphaFilteredYoFrameVector2d extends YoFrameVector2d +public class AlphaFilteredYoFrameVector2d extends YoFrameVector2D { private final AlphaFilteredYoVariable x, y; @@ -46,7 +46,7 @@ public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(St return ret; } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, YoFrameVector2d unfilteredVector) + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, YoFrameVector2D unfilteredVector) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); @@ -58,12 +58,12 @@ public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(St } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, YoFrameVector2d unfilteredVector) + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, YoFrameVector2D unfilteredVector) { return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, unfilteredVector); } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, DoubleProvider alpha, YoFrameVector2d unfilteredVector) + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, DoubleProvider alpha, YoFrameVector2D unfilteredVector) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, alpha, unfilteredVector.getYoX()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java index 01c22b1e..608da21b 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java @@ -2,16 +2,16 @@ import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.robotics.math.frames.YoFramePoint; +import us.ihmc.yoVariables.variable.YoFramePoint3D; +import us.ihmc.yoVariables.variable.YoFrameVector3D; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.robotics.math.frames.YoFrameVector; -public class BacklashCompensatingVelocityYoFrameVector extends YoFrameVector +public class BacklashCompensatingVelocityYoFrameVector extends YoFrameVector3D { private final BacklashCompensatingVelocityYoVariable xDot, yDot, zDot; public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensatingVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, YoDouble slopTime, - YoVariableRegistry registry, YoFrameVector yoFrameVectorToDifferentiate) + YoVariableRegistry registry, YoFrameVector3D yoFrameVectorToDifferentiate) { BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoX(), dt, slopTime, registry); @@ -26,7 +26,7 @@ public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensati } public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensatingVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, YoDouble slopTime, - YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) + YoVariableRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) { BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoX(), dt, slopTime, registry); @@ -41,7 +41,7 @@ public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensati } private BacklashCompensatingVelocityYoFrameVector(BacklashCompensatingVelocityYoVariable xDot, BacklashCompensatingVelocityYoVariable yDot, BacklashCompensatingVelocityYoVariable zDot, - YoVariableRegistry registry, YoFrameVector yoFrameVectorToDifferentiate) + YoVariableRegistry registry, YoFrameVector3D yoFrameVectorToDifferentiate) { super(xDot, yDot, zDot, yoFrameVectorToDifferentiate.getReferenceFrame()); @@ -51,7 +51,7 @@ private BacklashCompensatingVelocityYoFrameVector(BacklashCompensatingVelocityYo } private BacklashCompensatingVelocityYoFrameVector(BacklashCompensatingVelocityYoVariable xDot, BacklashCompensatingVelocityYoVariable yDot, BacklashCompensatingVelocityYoVariable zDot, - YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) + YoVariableRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) { super(xDot, yDot, zDot, yoFramePointToDifferentiate.getReferenceFrame()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java index 807eb68d..bede1b2c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java @@ -4,10 +4,10 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.robotics.math.frames.YoFramePoint2d; +import us.ihmc.yoVariables.variable.YoFramePoint2D; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -public class BetaFilteredYoFramePoint2d extends YoFramePoint2d +public class BetaFilteredYoFramePoint2d extends YoFramePoint2D { private final BetaFilteredYoVariable x, y; @@ -41,7 +41,7 @@ public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String return ret; } - public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFramePoint2d unfilteredPoint) + public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFramePoint2D unfilteredPoint) { // beta is a int BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoX()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java index 607bbc78..681a9a5e 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java @@ -4,10 +4,10 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple2D.Vector2D; import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoFrameVector2D; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.robotics.math.frames.YoFrameVector2d; -public class BetaFilteredYoFrameVector2d extends YoFrameVector2d +public class BetaFilteredYoFrameVector2d extends YoFrameVector2D { private final BetaFilteredYoVariable x, y; @@ -30,7 +30,7 @@ public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(Stri return ret; } - public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFrameVector2d unfilteredVector) + public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFrameVector2D unfilteredVector) { // beta is a int BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoX()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java index 7667ff72..e33a759a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java @@ -2,13 +2,13 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.robotics.math.frames.YoFrameTuple; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.robotics.math.frames.YoFrameVector; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFrameTuple3D; +import us.ihmc.yoVariables.variable.YoFrameVector3D; -public class DeadzoneYoFrameVector extends YoFrameVector implements ProcessingYoVariable +public class DeadzoneYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable { private final DeadzoneYoVariable x, y, z; @@ -37,12 +37,12 @@ public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefi return ret; } - public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, YoVariableRegistry registry, YoDouble deadzoneSize, YoFrameTuple rawTuple) + public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, YoVariableRegistry registry, YoDouble deadzoneSize, YoFrameTuple3D rawTuple) { return createDeadzoneYoFrameVector(namePrefix, "", registry, deadzoneSize, rawTuple); } - public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble deadzoneSize, YoFrameTuple rawTuple) + public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble deadzoneSize, YoFrameTuple3D rawTuple) { DeadzoneYoVariable x = new DeadzoneYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), rawTuple.getYoX(), deadzoneSize, registry); DeadzoneYoVariable y = new DeadzoneYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), rawTuple.getYoY(), deadzoneSize, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java index 70a895ad..0ad3dc8c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java @@ -3,11 +3,11 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.robotics.math.frames.YoFramePoint; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.robotics.math.frames.YoFrameVector; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFramePoint3D; +import us.ihmc.yoVariables.variable.YoFrameVector3D; /** *

FilteredVelocityYoFrameVector

@@ -19,12 +19,12 @@ * @author IHMC Biped Team * @version 1.0 */ -public class FilteredVelocityYoFrameVector extends YoFrameVector +public class FilteredVelocityYoFrameVector extends YoFrameVector3D { private final FilteredVelocityYoVariable xDot, yDot, zDot; public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, - YoVariableRegistry registry, YoFrameVector yoFrameVectorToDifferentiate) + YoVariableRegistry registry, YoFrameVector3D yoFrameVectorToDifferentiate) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoX(), dt, registry); @@ -40,7 +40,7 @@ public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector( } public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, - YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) + YoVariableRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoX(), dt, registry); @@ -78,7 +78,7 @@ private FilteredVelocityYoFrameVector(FilteredVelocityYoVariable xDot, FilteredV } private FilteredVelocityYoFrameVector(FilteredVelocityYoVariable xDot, FilteredVelocityYoVariable yDot, FilteredVelocityYoVariable zDot, - YoDouble alpha, double dt, YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) + YoDouble alpha, double dt, YoVariableRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) { super(xDot, yDot, zDot, yoFramePointToDifferentiate.getReferenceFrame()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java index 10a2a55f..92638f36 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java @@ -3,26 +3,26 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; -import us.ihmc.robotics.math.frames.YoFramePoint; -import us.ihmc.robotics.math.frames.YoFrameTuple2d; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.robotics.math.frames.YoFrameVector2d; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFramePoint3D; +import us.ihmc.yoVariables.variable.YoFrameTuple2D; +import us.ihmc.yoVariables.variable.YoFrameVector2D; -public class FilteredVelocityYoFrameVector2d extends YoFrameVector2d +public class FilteredVelocityYoFrameVector2d extends YoFrameVector2D { private final FilteredVelocityYoVariable xDot, yDot; public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, YoDouble alpha, double dt, - YoVariableRegistry registry, YoFrameTuple2d yoFrameVectorToDifferentiate) + YoVariableRegistry registry, YoFrameTuple2D yoFrameVectorToDifferentiate) { return createFilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, "", alpha, dt, registry, yoFrameVectorToDifferentiate); } public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoDouble alpha, double dt, YoVariableRegistry registry, - YoFrameTuple2d yoFrameVectorToDifferentiate) + YoFrameTuple2D yoFrameVectorToDifferentiate) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, alpha, yoFrameVectorToDifferentiate.getYoX(), dt, registry); @@ -56,7 +56,7 @@ private FilteredVelocityYoFrameVector2d(FilteredVelocityYoVariable xDot, Filtere } private FilteredVelocityYoFrameVector2d(FilteredVelocityYoVariable xDot, FilteredVelocityYoVariable yDot, YoDouble alpha, double dt, - YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) + YoVariableRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) { super(xDot, yDot, yoFramePointToDifferentiate.getReferenceFrame()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java index aac85152..528ed7e2 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java @@ -7,12 +7,12 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; -import us.ihmc.robotics.math.frames.YoFrameQuaternion; -import us.ihmc.robotics.math.frames.YoFrameVector; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoFrameQuaternion; +import us.ihmc.yoVariables.variable.YoFrameVector3D; -public class FiniteDifferenceAngularVelocityYoFrameVector extends YoFrameVector +public class FiniteDifferenceAngularVelocityYoFrameVector extends YoFrameVector3D { private final YoFrameQuaternion orientation; private final YoFrameQuaternion orientationPreviousValue; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java index d3d09342..1e86932d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java @@ -5,17 +5,17 @@ import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; -import us.ihmc.robotics.math.frames.YoFrameOrientation; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFrameYawPitchRoll; -public class RateLimitedYoFrameOrientation extends YoFrameOrientation +public class RateLimitedYoFrameOrientation extends YoFrameYawPitchRoll { private final DoubleProvider maxRateVariable; - private final YoFrameOrientation rawOrientation; + private final YoFrameYawPitchRoll rawOrientation; private final YoBoolean limited; private final YoBoolean hasBeenCalled; private final double dt; @@ -28,7 +28,7 @@ private static DoubleProvider createMaxRateYoDouble(String namePrefix, String na } public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, - YoFrameOrientation rawOrientation) + YoFrameYawPitchRoll rawOrientation) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawOrientation, rawOrientation.getReferenceFrame()); } @@ -40,7 +40,7 @@ public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVar } public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, - YoFrameOrientation rawOrientation) + YoFrameYawPitchRoll rawOrientation) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawOrientation, rawOrientation.getReferenceFrame()); @@ -53,7 +53,7 @@ public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVar } private RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, - YoFrameOrientation rawOrientation, ReferenceFrame referenceFrame) + YoFrameYawPitchRoll rawOrientation, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); @@ -88,7 +88,7 @@ public void update() update(rawOrientation); } - public void update(YoFrameOrientation yoFrameVectorUnfiltered) + public void update(YoFrameYawPitchRoll yoFrameVectorUnfiltered) { checkReferenceFrameMatch(yoFrameVectorUnfiltered); update(yoFrameVectorUnfiltered.getYaw().getDoubleValue(), yoFrameVectorUnfiltered.getPitch().getDoubleValue(), diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java index 089cc9f5..ef258e76 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java @@ -4,13 +4,13 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.robotics.math.frames.YoFramePoint; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFramePoint3D; -public class RateLimitedYoFramePoint extends YoFramePoint +public class RateLimitedYoFramePoint extends YoFramePoint3D { private final DoubleProvider maxRateVariable; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java index 4b45ec5c..90c34f49 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java @@ -6,11 +6,11 @@ import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; -import us.ihmc.robotics.math.frames.YoFrameQuaternion; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFrameQuaternion; public class RateLimitedYoFrameQuaternion extends YoFrameQuaternion { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java index 1492a13e..c83c50b6 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java @@ -4,13 +4,13 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.robotics.math.frames.YoFrameVector; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFrameVector3D; -public class RateLimitedYoFrameVector extends YoFrameVector +public class RateLimitedYoFrameVector extends YoFrameVector3D { private final DoubleProvider maxRateVariable; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java index a6e1833f..82e07d17 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java @@ -5,12 +5,12 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.robotics.math.frames.YoFrameVector2d; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFrameVector2D; -public class RateLimitedYoFrameVector2d extends YoFrameVector2d +public class RateLimitedYoFrameVector2d extends YoFrameVector2D { private final RateLimitedYoVariable x, y; @@ -46,7 +46,7 @@ public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, - double maxRate, double dt, YoFrameVector2d unfilteredVector) + double maxRate, double dt, YoFrameVector2D unfilteredVector) { RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); @@ -58,7 +58,7 @@ public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, - YoDouble maxRate, double dt, YoFrameVector2d unfilteredVector) + YoDouble maxRate, double dt, YoFrameVector2D unfilteredVector) { RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java index 92ba898a..51bb8535 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java @@ -4,10 +4,10 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoFrameVector3D; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.robotics.math.frames.YoFrameVector; -public class SecondOrderFilteredYoFrameVector extends YoFrameVector implements ProcessingYoVariable +public class SecondOrderFilteredYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable { private final SecondOrderFilteredYoVariable x, y, z; @@ -40,7 +40,7 @@ public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameV } public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, - double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType, YoFrameVector unfilteredVector) + double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType, YoFrameVector3D unfilteredVector) { SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, registry, naturalFrequencyInHz, dampingRatio, filterType); @@ -48,7 +48,7 @@ public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameV } public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, - double dt, SecondOrderFilteredYoVariableParameters parameters, YoFrameVector unfilteredVector) + double dt, SecondOrderFilteredYoVariableParameters parameters, YoFrameVector3D unfilteredVector) { SecondOrderFilteredYoVariable x, y, z; x = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoX()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java index e2550a48..f09e918e 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java @@ -8,9 +8,9 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.robotics.math.frames.YoFrameVector; +import us.ihmc.yoVariables.variable.YoFrameVector3D; -public class SimpleMovingAverageFilteredYoFrameVector extends YoFrameVector implements ProcessingYoVariable +public class SimpleMovingAverageFilteredYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable { private final SimpleMovingAverageFilteredYoVariable x, y, z; @@ -39,7 +39,7 @@ public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverage } public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, - YoFrameVector unfilteredVector, YoVariableRegistry registry) + YoFrameVector3D unfilteredVector, YoVariableRegistry registry) { String xName = createXName(namePrefix, nameSuffix); String yName = createYName(namePrefix, nameSuffix); From 7134ceeeac804cd9df8809ea314dfdb85e391e7a Mon Sep 17 00:00:00 2001 From: gwiedebach Date: Mon, 16 Apr 2018 12:30:09 -0500 Subject: [PATCH 08/47] Switched some processing yo variables to use DoubleProviders for their parameters. Cleanup: made alpha filtered yo variable not call getValue() on alpha provider on first update. That way it can be called in the constructor to initialize the filter. Switched yovariables to DoubleProviders for the rate limiter, allowing to pass in a parameter instead of a yovariable for the ratelimit. --- .../math/filters/AlphaFilteredYoVariable.java | 9 ++++-- .../filters/BacklashProcessingYoVariable.java | 21 ++++++------ .../filters/FilteredVelocityYoVariable.java | 14 ++++---- .../filters/RateLimitedYoFrameVector2d.java | 6 ---- .../math/filters/RateLimitedYoVariable.java | 32 +++++++++---------- ...acklashCompensatingVelocityYoVariable.java | 30 +++++++---------- 6 files changed, 49 insertions(+), 63 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java index 0d59f6a5..d3aa3705 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java @@ -93,6 +93,7 @@ public void reset() hasBeenCalled.set(false); } + @Override public void update() { if (position == null) @@ -111,9 +112,11 @@ public void update(double currentPosition) hasBeenCalled.set(true); set(currentPosition); } - - double alpha = alphaVariable.getValue(); - set(alpha * getDoubleValue() + (1.0 - alpha) * currentPosition); + else + { + double alpha = alphaVariable.getValue(); + set(alpha * getDoubleValue() + (1.0 - alpha) * currentPosition); + } } /** diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java index 6875c01d..a5487305 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java @@ -1,6 +1,7 @@ package us.ihmc.robotics.math.filters; import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -17,18 +18,18 @@ public class BacklashProcessingYoVariable extends YoDouble implements Processing private final YoBoolean hasBeenCalled; private final YoEnum backlashState; - private final YoDouble slopTime; + private final DoubleProvider slopTime; private final YoDouble timeSinceSloppy; private final double dt; - public BacklashProcessingYoVariable(String name, String description, double dt, YoDouble slopTime, YoVariableRegistry registry) + public BacklashProcessingYoVariable(String name, String description, double dt, DoubleProvider slopTime, YoVariableRegistry registry) { this(name, description, null, dt, slopTime, registry); } - public BacklashProcessingYoVariable(String name, String description, YoDouble velocityVariable, double dt, YoDouble slopTime, + public BacklashProcessingYoVariable(String name, String description, YoDouble velocityVariable, double dt, DoubleProvider slopTime, YoVariableRegistry registry) { super(name, description, registry); @@ -54,6 +55,7 @@ public void reset() backlashState.set(null); } + @Override public void update() { if (velocity == null) @@ -111,7 +113,7 @@ public void update(double currentVelocity) timeSinceSloppy.set(0.0); backlashState.set(BacklashState.FORWARD_SLOP); } - else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) + else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) { backlashState.set(BacklashState.BACKWARD_OK); } @@ -126,7 +128,7 @@ else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) timeSinceSloppy.set(0.0); backlashState.set(BacklashState.BACKWARD_SLOP); } - else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) + else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) { backlashState.set(BacklashState.FORWARD_OK); } @@ -135,19 +137,14 @@ else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) } } - double percent = timeSinceSloppy.getDoubleValue() / slopTime.getDoubleValue(); + double percent = timeSinceSloppy.getDoubleValue() / slopTime.getValue(); percent = MathTools.clamp(percent, 0.0, 1.0); - if (Double.isNaN(percent) || slopTime.getDoubleValue() < dt) + if (Double.isNaN(percent) || slopTime.getValue() < dt) percent = 1.0; this.set(percent * currentVelocity); } - public void setSlopTime(double slopTime) - { - this.slopTime.set(slopTime); - } - private enum BacklashState { BACKWARD_OK, FORWARD_OK, BACKWARD_SLOP, FORWARD_SLOP; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java index a47053db..8e835aa5 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java @@ -1,9 +1,10 @@ package us.ihmc.robotics.math.filters; +import us.ihmc.robotics.geometry.AngleTools; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.robotics.geometry.AngleTools; /** * @author jrebula @@ -25,7 +26,7 @@ public class FilteredVelocityYoVariable extends YoDouble implements ProcessingYo private double alphaDouble; private final double dt; - private final YoDouble alphaVariable; + private final DoubleProvider alphaVariable; private final YoDouble position; // private double lastPosition; @@ -65,7 +66,7 @@ public FilteredVelocityYoVariable(String name, String description, double alpha, reset(); } - public FilteredVelocityYoVariable(String name, String description, YoDouble alphaVariable, YoDouble positionVariable, double dt, YoVariableRegistry registry) + public FilteredVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, YoDouble positionVariable, double dt, YoVariableRegistry registry) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); @@ -81,7 +82,7 @@ public FilteredVelocityYoVariable(String name, String description, YoDouble alph reset(); } - public FilteredVelocityYoVariable(String name, String description, YoDouble alphaVariable, double dt, YoVariableRegistry registry) + public FilteredVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, double dt, YoVariableRegistry registry) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); @@ -102,6 +103,7 @@ public void reset() hasBeenCalled.set(false); } + @Override public void update() { if (position == null) @@ -161,7 +163,7 @@ private void updateUsingDifference(double difference) double previousFilteredDerivative = getDoubleValue(); double currentRawDerivative = difference / dt; - double alpha = alphaVariable == null ? alphaDouble : alphaVariable.getDoubleValue(); + double alpha = alphaVariable == null ? alphaDouble : alphaVariable.getValue(); set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); } @@ -173,7 +175,7 @@ public void setAlpha(double alpha) } else { - alphaVariable.set(alpha); + throw new RuntimeException("A double provider was used to construct this filtered variable. Modyfy the value of that provider directly."); } } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java index 82e07d17..3a04be0d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java @@ -92,12 +92,6 @@ public void update(FrameTuple2DReadOnly vector2dUnfiltered) y.update(vector2dUnfiltered.getY()); } - public void setMaxRate(double maxRate) - { - x.setMaxRate(maxRate); - y.setMaxRate(maxRate); - } - public void reset() { x.reset(); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java index 35f4a687..3d889b19 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java @@ -1,12 +1,13 @@ package us.ihmc.robotics.math.filters; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; public class RateLimitedYoVariable extends YoDouble { - private final YoDouble maxRateVariable; + private final DoubleProvider maxRateVariable; private final YoDouble position; private final YoBoolean limited; @@ -21,9 +22,10 @@ public RateLimitedYoVariable(String name, YoVariableRegistry registry, double ma this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); this.limited = new YoBoolean(name + "Limited", registry); - this.maxRateVariable = new YoDouble(name + "MaxRate", registry); - this.maxRateVariable.set(maxRate); - + YoDouble maxRateVariable = new YoDouble(name + "MaxRate", registry); + maxRateVariable.set(maxRate); + this.maxRateVariable = maxRateVariable; + this.position = null; this.dt = dt; @@ -31,7 +33,7 @@ public RateLimitedYoVariable(String name, YoVariableRegistry registry, double ma reset(); } - public RateLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxRateVariable, double dt) + public RateLimitedYoVariable(String name, YoVariableRegistry registry, DoubleProvider maxRateVariable, double dt) { super(name, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); @@ -52,15 +54,16 @@ public RateLimitedYoVariable(String name, YoVariableRegistry registry, double ma position = positionVariable; - this.maxRateVariable = new YoDouble(name + "MaxRate", registry); - this.maxRateVariable.set(maxRate); - + YoDouble maxRateVariable = new YoDouble(name + "MaxRate", registry); + maxRateVariable.set(maxRate); + this.maxRateVariable = maxRateVariable; + this.dt = dt; reset(); } - public RateLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxRateVariable, YoDouble positionVariable, double dt) + public RateLimitedYoVariable(String name, YoVariableRegistry registry, DoubleProvider maxRateVariable, YoDouble positionVariable, double dt) { super(name, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); @@ -74,11 +77,6 @@ public RateLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble reset(); } - public void setMaxRate(double maxRate) - { - this.maxRateVariable.set(maxRate); - } - public void reset() { hasBeenCalled.set(false); @@ -103,13 +101,13 @@ public void update(double currentPosition) set(currentPosition); } - if (maxRateVariable.getDoubleValue() < 0) + if (maxRateVariable.getValue() < 0) throw new RuntimeException("The maxRate parameter in the RateLimitedYoVariable cannot be negative."); double difference = currentPosition - getDoubleValue(); - if (Math.abs(difference) > maxRateVariable.getDoubleValue() * dt) + if (Math.abs(difference) > maxRateVariable.getValue() * dt) { - difference = Math.signum(difference) * maxRateVariable.getDoubleValue() * dt; + difference = Math.signum(difference) * maxRateVariable.getValue() * dt; this.limited.set(true); } else diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java index e1952293..0ab38834 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java @@ -1,6 +1,7 @@ package us.ihmc.robotics.math.filters; import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -17,20 +18,20 @@ public class RevisedBacklashCompensatingVelocityYoVariable extends YoDouble impl private final double dt; private final FilteredVelocityYoVariable finiteDifferenceVelocity; - - private final YoDouble alphaVariable; + + private final DoubleProvider alphaVariable; private final YoDouble position; private final YoDouble lastPosition; private final YoBoolean hasBeenCalled; private final YoEnum backlashState; - private final YoDouble slopTime; + private final DoubleProvider slopTime; private final YoDouble timeSinceSloppy; - public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, YoDouble positionVariable, - double dt, YoDouble slopTime, YoVariableRegistry registry) + public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, YoDouble positionVariable, + double dt, DoubleProvider slopTime, YoVariableRegistry registry) { super(name, description, registry); @@ -86,6 +87,7 @@ public void reset() backlashState.set(null); } + @Override public void update() { if (position == null) @@ -151,7 +153,7 @@ public void update(double currentPosition) // this.set(0.0); backlashState.set(BacklashState.FORWARD_SLOP); } - else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) + else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) { backlashState.set(BacklashState.BACKWARD_OK); } @@ -167,7 +169,7 @@ else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) // this.set(0.0); backlashState.set(BacklashState.BACKWARD_SLOP); } - else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) + else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) { backlashState.set(BacklashState.FORWARD_OK); } @@ -178,7 +180,7 @@ else if (timeSinceSloppy.getDoubleValue() > slopTime.getDoubleValue()) double difference = currentPosition - lastPosition.getDoubleValue(); - double percent = timeSinceSloppy.getDoubleValue() / slopTime.getDoubleValue(); + double percent = timeSinceSloppy.getDoubleValue() / slopTime.getValue(); percent = MathTools.clamp(percent, 0.0, 1.0); if (Double.isNaN(percent)) percent = 1.0; @@ -195,19 +197,9 @@ private void updateUsingDifference(double difference) double previousFilteredDerivative = getDoubleValue(); double currentRawDerivative = difference / dt; - double alpha = alphaVariable.getDoubleValue(); + double alpha = alphaVariable.getValue(); this.set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); } - public void setAlpha(double alpha) - { - this.alphaVariable.set(alpha); - } - - public void setSlopTime(double slopTime) - { - this.slopTime.set(slopTime); - } - private enum BacklashState {BACKWARD_OK, FORWARD_OK, BACKWARD_SLOP, FORWARD_SLOP;} } From 73bd83c8f2504134381ef6bf3237e0e2b97407d4 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Wed, 5 Sep 2018 17:09:58 -0500 Subject: [PATCH 09/47] Changed argument type to use its read-only super type. --- .../robotics/math/filters/RateLimitedYoFrameQuaternion.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java index 90c34f49..6803d04d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java @@ -1,6 +1,5 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.euclid.referenceFrame.FrameQuaternion; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; import us.ihmc.euclid.tuple3D.Vector3D; @@ -95,10 +94,10 @@ public void update() update(rawQuaternion); } - public void update(FrameQuaternion frameOrientationUnfiltered) + public void update(FrameQuaternionReadOnly frameOrientationUnfiltered) { checkReferenceFrameMatch(frameOrientationUnfiltered); - update(frameOrientationUnfiltered); + update((QuaternionReadOnly) frameOrientationUnfiltered); } private final Quaternion difference = new Quaternion(); From c3c88b915b943d4c158641cf8de46ad2d4c2cf0f Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Thu, 6 Sep 2018 11:16:54 -0500 Subject: [PATCH 10/47] Removed duplicated "hasBeenCalled" variables from some filters. --- .../filters/AlphaFilteredYoFramePoint.java | 183 +++++++++--------- .../filters/AlphaFilteredYoFramePoint2d.java | 160 ++++++++++----- .../filters/AlphaFilteredYoFrameVector.java | 170 ++++++++-------- .../filters/AlphaFilteredYoFrameVector2d.java | 173 +++++++++++------ .../math/filters/AlphaFilteredYoVariable.java | 2 +- .../FilteredVelocityYoFrameVector.java | 156 ++++++++------- .../FilteredVelocityYoFrameVector2d.java | 141 +++++++++----- 7 files changed, 586 insertions(+), 399 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java index 0495f951..df18443a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java @@ -1,139 +1,144 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.euclid.referenceFrame.FramePoint3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoFramePoint3D; public class AlphaFilteredYoFramePoint extends YoFramePoint3D { - private final AlphaFilteredYoVariable x, y, z; + private final DoubleProvider alphaProvider; - private AlphaFilteredYoFramePoint(AlphaFilteredYoVariable x, AlphaFilteredYoVariable y, AlphaFilteredYoVariable z, ReferenceFrame referenceFrame) - { - super(x, y, z, referenceFrame); - - this.x = x; - this.y = y; - this.z = z; - } + private final FrameTuple3DReadOnly position; + private final YoBoolean hasBeenCalled; + /** + * @deprecated Use + * {@link #AlphaFilteredYoFramePoint(String, String, YoVariableRegistry, double, ReferenceFrame)} + * instead. + */ + @Deprecated public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, - ReferenceFrame referenceFrame) + ReferenceFrame referenceFrame) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); - AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha); - - AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, referenceFrame); - - return ret; + return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, referenceFrame); } + /** + * @deprecated Use + * {@link #AlphaFilteredYoFramePoint(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * instead. + */ + @Deprecated public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); - AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha); - - AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, referenceFrame); - - return ret; + return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, referenceFrame); } - + /** + * @deprecated Use + * {@link #AlphaFilteredYoFramePoint(String, String, YoVariableRegistry, double, FrameTuple3DReadOnly)} + * instead. + */ + @Deprecated public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, - YoFramePoint3D unfilteredPoint) + FrameTuple3DReadOnly unfilteredFrameTuple3D) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoY()); - AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoZ()); - - AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); - - return ret; + return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); } - + /** + * @deprecated Use + * {@link #AlphaFilteredYoFramePoint(String, String, YoVariableRegistry, DoubleProvider, FrameTuple3DReadOnly)} + * instead. + */ + @Deprecated public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, - DoubleProvider alpha, YoFramePoint3D unfilteredPoint) + DoubleProvider alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) { - // alpha is a YoVariable - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoY()); - AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoZ()); + return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); + } - AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); + public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); + } - return ret; + public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); } - - public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, - double alphaX, double alphaY, double alphaZ, YoFramePoint3D unfilteredPoint) + + public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + FrameTuple3DReadOnly unfilteredFrameTuple3D) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alphaX, unfilteredPoint.getYoX()); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alphaY, unfilteredPoint.getYoY()); - AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alphaZ, unfilteredPoint.getYoZ()); - - AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); - - return ret; + this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), + unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); } - - public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, - DoubleProvider alphaX, DoubleProvider alphaY, DoubleProvider alphaZ, YoFramePoint3D unfilteredPoint) + + public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, + FrameTuple3DReadOnly unfilteredFrameTuple3D) { - // alpha is a YoVariable - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alphaX, unfilteredPoint.getYoX()); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alphaY, unfilteredPoint.getYoY()); - AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alphaZ, unfilteredPoint.getYoZ()); - - AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); - - return ret; + this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); } - public void update() + private AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + FrameTuple3DReadOnly unfilteredFrameTuple3D) { - x.update(); - y.update(); - z.update(); + super(namePrefix, nameSuffix, referenceFrame, registry); + + alphaProvider = alpha; + + position = unfilteredFrameTuple3D; + hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); + reset(); } - public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + public void reset() { - x.update(xUnfiltered); - y.update(yUnfiltered); - z.update(zUnfiltered); + hasBeenCalled.set(false); } - public void update(Point3D pointUnfiltered) + public void update() { - x.update(pointUnfiltered.getX()); - y.update(pointUnfiltered.getY()); - z.update(pointUnfiltered.getZ()); + if (position == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position); } - public void update(FramePoint3D pointUnfiltered) + public void update(FrameTuple3DReadOnly unfilteredFrameTuple3D) { - checkReferenceFrameMatch(pointUnfiltered); - x.update(pointUnfiltered.getX()); - y.update(pointUnfiltered.getY()); - z.update(pointUnfiltered.getZ()); + checkReferenceFrameMatch(unfilteredFrameTuple3D); + update((Tuple3DReadOnly) unfilteredFrameTuple3D); } - public void reset() + public void update(Tuple3DReadOnly unfilteredTuple3D) + { + update(unfilteredTuple3D.getX(), unfilteredTuple3D.getY(), unfilteredTuple3D.getZ()); + } + + private final Point3D unfilteredPoint3D = new Point3D(); + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) { - x.reset(); - y.reset(); - z.reset(); + if (!hasBeenCalled.getValue()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered, zUnfiltered); + } + else + { + unfilteredPoint3D.set(xUnfiltered, yUnfiltered, zUnfiltered); + interpolate(unfilteredPoint3D, this, alphaProvider.getValue()); + } } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java index ba3506c0..0e2fe1b5 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java @@ -1,102 +1,156 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.euclid.referenceFrame.FramePoint2D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; import us.ihmc.euclid.tuple2D.Point2D; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoFramePoint2D; public class AlphaFilteredYoFramePoint2d extends YoFramePoint2D { - private final AlphaFilteredYoVariable x, y; - - private AlphaFilteredYoFramePoint2d(AlphaFilteredYoVariable x, AlphaFilteredYoVariable y, ReferenceFrame referenceFrame) + private final DoubleProvider alphaProvider; + + private final FrameTuple2DReadOnly position; + private final YoBoolean hasBeenCalled; + + /** + * @deprecated Use + * {@link #AlphaFilteredYoFramePoint2d(String, String, YoVariableRegistry, double, ReferenceFrame)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + ReferenceFrame referenceFrame) { - super(x, y, referenceFrame); - - this.x = x; - this.y = y; + return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); } - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + /** + * @deprecated Use + * {@link #AlphaFilteredYoFramePoint2d(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + DoubleProvider alpha, ReferenceFrame referenceFrame) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); - - AlphaFilteredYoFramePoint2d ret = new AlphaFilteredYoFramePoint2d(x, y, referenceFrame); - - return ret; + return createAlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); } - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + /** + * @deprecated Use + * {@link #AlphaFilteredYoFramePoint2d(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, String description, + YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { - return createAlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); + return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); } - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + /** + * @deprecated Use + * {@link #AlphaFilteredYoFramePoint2d(String, String, YoVariableRegistry, double, FrameTuple2DReadOnly)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + FrameTuple2DReadOnly unfilteredFrameTuple2D) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, alpha); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, registry, alpha); + return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); + } - AlphaFilteredYoFramePoint2d ret = new AlphaFilteredYoFramePoint2d(x, y, referenceFrame); + /** + * @deprecated Use + * {@link #AlphaFilteredYoFramePoint2d(String, String, YoVariableRegistry, DoubleProvider, FrameTuple2DReadOnly)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + DoubleProvider alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) + { + return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); + } - return ret; + public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); } - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, YoFramePoint2D unfilteredPoint) + public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoY()); + this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); + } - AlphaFilteredYoFramePoint2d ret = new AlphaFilteredYoFramePoint2d(x, y, unfilteredPoint.getReferenceFrame()); + public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + FrameTuple2DReadOnly unfilteredFrameTuple2D) + { + this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), + unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); + } - return ret; + public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, + FrameTuple2DReadOnly unfilteredFrameTuple2D) + { + this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); } - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, YoFramePoint2D unfilteredPoint) + private AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + FrameTuple2DReadOnly unfilteredFrameTuple2D) { - // alpha is a YoVariable - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoY()); + super(namePrefix, nameSuffix, referenceFrame, registry); - AlphaFilteredYoFramePoint2d ret = new AlphaFilteredYoFramePoint2d(x, y, unfilteredPoint.getReferenceFrame()); + alphaProvider = alpha; - return ret; + position = unfilteredFrameTuple2D; + hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); + reset(); } - public void update() + public void reset() { - x.update(); - y.update(); + hasBeenCalled.set(false); } - public void update(double xUnfiltered, double yUnfiltered) + public void update() { - x.update(xUnfiltered); - y.update(yUnfiltered); + if (position == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position); } - public void update(Point2D point2dUnfiltered) + public void update(FrameTuple2DReadOnly unfilteredFrameTuple2D) { - x.update(point2dUnfiltered.getX()); - y.update(point2dUnfiltered.getY()); + checkReferenceFrameMatch(unfilteredFrameTuple2D); + update((Tuple2DReadOnly) unfilteredFrameTuple2D); } - public void update(FramePoint2D point2dUnfiltered) + public void update(Tuple2DReadOnly unfilteredTuple2D) { - checkReferenceFrameMatch(point2dUnfiltered); - x.update(point2dUnfiltered.getX()); - y.update(point2dUnfiltered.getY()); + update(unfilteredTuple2D.getX(), unfilteredTuple2D.getY()); } - public void reset() + private final Point2D unfilteredPoint2D = new Point2D(); + + public void update(double xUnfiltered, double yUnfiltered) { - x.reset(); - y.reset(); + if (!hasBeenCalled.getValue()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered); + } + else + { + unfilteredPoint2D.set(xUnfiltered, yUnfiltered); + interpolate(unfilteredPoint2D, this, alphaProvider.getValue()); + } } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java index 554c9e7c..0c0fcd2c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java @@ -1,127 +1,145 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoFramePoint3D; +import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoFrameVector3D; public class AlphaFilteredYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable { - private final AlphaFilteredYoVariable x, y, z; + private final DoubleProvider alphaProvider; - private AlphaFilteredYoFrameVector(AlphaFilteredYoVariable x, AlphaFilteredYoVariable y, AlphaFilteredYoVariable z, ReferenceFrame referenceFrame) - { - super(x, y, z, referenceFrame); - - this.x = x; - this.y = y; - this.z = z; - } + private final FrameTuple3DReadOnly position; + private final YoBoolean hasBeenCalled; + /** + * @deprecated Use + * {@link #AlphaFilteredYoFrameVector(String, String, YoVariableRegistry, double, ReferenceFrame)} + * instead. + */ + @Deprecated public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, - ReferenceFrame referenceFrame) + ReferenceFrame referenceFrame) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); - AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha); - - AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, referenceFrame); - - return ret; + return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, referenceFrame); } + /** + * @deprecated Use + * {@link #AlphaFilteredYoFrameVector(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * instead. + */ + @Deprecated public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, - DoubleProvider alpha, ReferenceFrame referenceFrame) + DoubleProvider alpha, ReferenceFrame referenceFrame) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); - AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha); - - AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, referenceFrame); - - return ret; + return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, referenceFrame); } - + /** + * @deprecated Use + * {@link #AlphaFilteredYoFrameVector(String, String, YoVariableRegistry, double, FrameTuple3DReadOnly)} + * instead. + */ + @Deprecated public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, - YoFrameVector3D unfilteredVector) + FrameTuple3DReadOnly unfilteredFrameTuple3D) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoY()); - AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoZ()); - - AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); + return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); + } - return ret; + /** + * @deprecated Use + * {@link #AlphaFilteredYoFrameVector(String, String, YoVariableRegistry, DoubleProvider, FrameTuple3DReadOnly)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + DoubleProvider alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) + { + return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); } + public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); + } - public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, - DoubleProvider alpha, YoFrameVector3D unfilteredVector) + public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { - // alpha is a YoVariable - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoY()); - AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoZ()); + this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); + } - AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); + public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + FrameTuple3DReadOnly unfilteredFrameTuple3D) + { + this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), + unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); + } - return ret; + public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, + FrameTuple3DReadOnly unfilteredFrameTuple3D) + { + this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); } - public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, - DoubleProvider alpha, YoFramePoint3D unfilteredPosition) + private AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + FrameTuple3DReadOnly unfilteredFrameTuple3D) { - // alpha is a YoVariable - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPosition.getYoX()); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPosition.getYoY()); - AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredPosition.getYoZ()); + super(namePrefix, nameSuffix, referenceFrame, registry); - AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, unfilteredPosition.getReferenceFrame()); + alphaProvider = alpha; - return ret; + position = unfilteredFrameTuple3D; + hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); + reset(); } - public void update() + public void reset() { - x.update(); - y.update(); - z.update(); + hasBeenCalled.set(false); } - public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + @Override + public void update() { - x.update(xUnfiltered); - y.update(yUnfiltered); - z.update(zUnfiltered); + if (position == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position); } - public void update(Vector3D vectorUnfiltered) + public void update(FrameTuple3DReadOnly unfilteredFrameTuple3D) { - x.update(vectorUnfiltered.getX()); - y.update(vectorUnfiltered.getY()); - z.update(vectorUnfiltered.getZ()); + checkReferenceFrameMatch(unfilteredFrameTuple3D); + update((Tuple3DReadOnly) unfilteredFrameTuple3D); } - public void update(FrameVector3D vectorUnfiltered) + public void update(Tuple3DReadOnly unfilteredTuple3D) { - checkReferenceFrameMatch(vectorUnfiltered); - x.update(vectorUnfiltered.getX()); - y.update(vectorUnfiltered.getY()); - z.update(vectorUnfiltered.getZ()); + update(unfilteredTuple3D.getX(), unfilteredTuple3D.getY(), unfilteredTuple3D.getZ()); } - public void reset() + private final Vector3D unfilteredVector3D = new Vector3D(); + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) { - x.reset(); - y.reset(); - z.reset(); + if (!hasBeenCalled.getValue()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered, zUnfiltered); + } + else + { + unfilteredVector3D.set(xUnfiltered, yUnfiltered, zUnfiltered); + interpolate(unfilteredVector3D, this, alphaProvider.getValue()); + } } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java index 06c9d8d5..6bba4040 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java @@ -1,107 +1,170 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.euclid.referenceFrame.FrameVector2D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; import us.ihmc.euclid.tuple2D.Vector2D; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoFrameVector2D; public class AlphaFilteredYoFrameVector2d extends YoFrameVector2D { - private final AlphaFilteredYoVariable x, y; - - private AlphaFilteredYoFrameVector2d(AlphaFilteredYoVariable x, AlphaFilteredYoVariable y, ReferenceFrame referenceFrame) + private final DoubleProvider alphaProvider; + + private final FrameTuple2DReadOnly position; + private final YoBoolean hasBeenCalled; + + /** + * @deprecated Use + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, double, ReferenceFrame)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + double alpha, ReferenceFrame referenceFrame) { - super(x, y, referenceFrame); - - this.x = x; - this.y = y; + return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + /** + * @deprecated Use + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + DoubleProvider alpha, ReferenceFrame referenceFrame) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha); - - AlphaFilteredYoFrameVector2d ret = new AlphaFilteredYoFrameVector2d(x, y, referenceFrame); - - return ret; + return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + /** + * @deprecated Use + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, + YoVariableRegistry registry, DoubleProvider alpha, + ReferenceFrame referenceFrame) { - return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); + return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + /** + * @deprecated Use + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, double, FrameTuple2DReadOnly)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + double alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) { - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, alpha); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, registry, alpha); - - AlphaFilteredYoFrameVector2d ret = new AlphaFilteredYoFrameVector2d(x, y, referenceFrame); + return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); + } - return ret; + /** + * @deprecated Use + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, DoubleProvider, FrameTuple2DReadOnly)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + DoubleProvider alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) + { + return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, unfilteredFrameTuple2D); } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, YoFrameVector2D unfilteredVector) + /** + * @deprecated Use + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, DoubleProvider, FrameTuple2DReadOnly)} + * instead. + */ + @Deprecated + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, + YoVariableRegistry registry, DoubleProvider alpha, + FrameTuple2DReadOnly unfilteredFrameTuple2D) { - // alpha is a double - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoX()); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredVector.getYoY()); + return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); + } - AlphaFilteredYoFrameVector2d ret = new AlphaFilteredYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); + public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); + } - return ret; + public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); } + public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + FrameTuple2DReadOnly unfilteredFrameTuple2D) + { + this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), + unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); + } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, YoFrameVector2D unfilteredVector) + public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, + FrameTuple2DReadOnly unfilteredFrameTuple2D) { - return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, unfilteredVector); + this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); } - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, DoubleProvider alpha, YoFrameVector2D unfilteredVector) + private AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + FrameTuple2DReadOnly unfilteredFrameTuple2D) { - // alpha is a YoVariable - AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, alpha, unfilteredVector.getYoX()); - AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, registry, alpha, unfilteredVector.getYoY()); + super(namePrefix, nameSuffix, referenceFrame, registry); - AlphaFilteredYoFrameVector2d ret = new AlphaFilteredYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); + alphaProvider = alpha; - return ret; + position = unfilteredFrameTuple2D; + hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); + reset(); } - public void update() + public void reset() { - x.update(); - y.update(); + hasBeenCalled.set(false); } - public void update(double xUnfiltered, double yUnfiltered) + public void update() { - x.update(xUnfiltered); - y.update(yUnfiltered); + if (position == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position); } - public void update(Vector2D vector2dUnfiltered) + public void update(FrameTuple2DReadOnly unfilteredFrameTuple2D) { - x.update(vector2dUnfiltered.getX()); - y.update(vector2dUnfiltered.getY()); + checkReferenceFrameMatch(unfilteredFrameTuple2D); + update((Tuple2DReadOnly) unfilteredFrameTuple2D); } - public void update(FrameVector2D vector2dUnfiltered) + public void update(Tuple2DReadOnly unfilteredTuple2D) { - checkReferenceFrameMatch(vector2dUnfiltered); - x.update(vector2dUnfiltered.getX()); - y.update(vector2dUnfiltered.getY()); + update(unfilteredTuple2D.getX(), unfilteredTuple2D.getY()); } - public void reset() + private final Vector2D unfilteredVector2D = new Vector2D(); + + public void update(double xUnfiltered, double yUnfiltered) { - x.reset(); - y.reset(); + if (!hasBeenCalled.getValue()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered); + } + else + { + unfilteredVector2D.set(xUnfiltered, yUnfiltered); + interpolate(unfilteredVector2D, this, alphaProvider.getValue()); + } } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java index d3aa3705..8e9aeac9 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java @@ -35,7 +35,7 @@ public class AlphaFilteredYoVariable extends YoDouble implements ProcessingYoVar private final YoDouble position; protected final YoBoolean hasBeenCalled; - private static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoVariableRegistry registry) + public static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoVariableRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "AlphaVariable", registry); maxRate.set(initialValue); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java index 0ad3dc8c..cf4c108d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java @@ -2,117 +2,129 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFramePoint3D; +import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoFrameVector3D; /** - *

FilteredVelocityYoFrameVector

+ *

+ * FilteredVelocityYoFrameVector + *

* - *

Differentiates and Filters a YoFrameVector to get its derivative.

+ *

+ * Differentiates and Filters a YoFrameVector to get its derivative. + *

* - *

IHMC

+ *

+ * IHMC + *

* * @author IHMC Biped Team * @version 1.0 */ public class FilteredVelocityYoFrameVector extends YoFrameVector3D { - private final FilteredVelocityYoVariable xDot, yDot, zDot; - - public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, - YoVariableRegistry registry, YoFrameVector3D yoFrameVectorToDifferentiate) + private final double dt; + private final DoubleProvider alphaProvider; + + private final YoBoolean hasBeenCalled; + private final FrameTuple3DReadOnly currentPosition; + private final YoFrameVector3D lastPosition; + + /** + * @deprecated Use + * {@link #FilteredVelocityYoFrameVector(String, String, DoubleProvider, double, YoVariableRegistry, FrameTuple3DReadOnly)} + * instead. + */ + @Deprecated + public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, + YoVariableRegistry registry, + FrameTuple3DReadOnly frameTuple3DToDifferentiate) { - FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, - yoFrameVectorToDifferentiate.getYoX(), dt, registry); - FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, - yoFrameVectorToDifferentiate.getYoY(), dt, registry); - FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, - yoFrameVectorToDifferentiate.getYoZ(), dt, registry); - - FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, - yoFrameVectorToDifferentiate.getReferenceFrame()); - - return ret; + return new FilteredVelocityYoFrameVector(namePrefix, nameSuffix, alpha, dt, registry, frameTuple3DToDifferentiate); } - public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, - YoVariableRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) + /** + * @deprecated Use + * {@link #FilteredVelocityYoFrameVector(String, String, DoubleProvider, double, YoVariableRegistry, ReferenceFrame)} + * instead. + */ + @Deprecated + public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, + YoVariableRegistry registry, ReferenceFrame referenceFrame) { - FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, - yoFramePointToDifferentiate.getYoX(), dt, registry); - FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, - yoFramePointToDifferentiate.getYoY(), dt, registry); - FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, - yoFramePointToDifferentiate.getYoZ(), dt, registry); - - FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, - yoFramePointToDifferentiate.getReferenceFrame()); - - return ret; + return new FilteredVelocityYoFrameVector(namePrefix, nameSuffix, alpha, dt, registry, referenceFrame); } - public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, - YoVariableRegistry registry, ReferenceFrame referenceFrame) + public FilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, + FrameTuple3DReadOnly frameTuple3DToDifferentiate) { - FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, dt, registry); - FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, dt, registry); - FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, dt, registry); - - FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, referenceFrame); - - return ret; + super(namePrefix, nameSuffix, frameTuple3DToDifferentiate.getReferenceFrame(), registry); + this.alphaProvider = alpha; + this.dt = dt; + + hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); + currentPosition = frameTuple3DToDifferentiate; + lastPosition = new YoFrameVector3D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); + reset(); } - private FilteredVelocityYoFrameVector(FilteredVelocityYoVariable xDot, FilteredVelocityYoVariable yDot, FilteredVelocityYoVariable zDot, - YoDouble alpha, double dt, YoVariableRegistry registry, ReferenceFrame referenceFrame) + public FilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, + ReferenceFrame referenceFrame) { - super(xDot, yDot, zDot, referenceFrame); + super(namePrefix, nameSuffix, referenceFrame, registry); - this.xDot = xDot; - this.yDot = yDot; - this.zDot = zDot; + this.alphaProvider = alpha; + this.dt = dt; + + hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); + currentPosition = null; + lastPosition = new YoFrameVector3D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); + reset(); } - private FilteredVelocityYoFrameVector(FilteredVelocityYoVariable xDot, FilteredVelocityYoVariable yDot, FilteredVelocityYoVariable zDot, - YoDouble alpha, double dt, YoVariableRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) + public void reset() { - super(xDot, yDot, zDot, yoFramePointToDifferentiate.getReferenceFrame()); - - this.xDot = xDot; - this.yDot = yDot; - this.zDot = zDot; + hasBeenCalled.set(false); } public void update() { - xDot.update(); - yDot.update(); - zDot.update(); - } + if (currentPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(FrameTuple3DReadOnly)"); + } - public void update(Tuple3DReadOnly tuple) - { - xDot.update(tuple.getX()); - yDot.update(tuple.getY()); - zDot.update(tuple.getZ()); + update(currentPosition); } public void update(FrameTuple3DReadOnly frameTuple) { checkReferenceFrameMatch(frameTuple); - xDot.update(frameTuple.getX()); - yDot.update(frameTuple.getY()); - zDot.update(frameTuple.getZ()); + update((Tuple3DReadOnly) frameTuple); } - public void reset() + private final Vector3D currentRawDerivative = new Vector3D(); + + public void update(Tuple3DReadOnly currentPosition) { - xDot.reset(); - yDot.reset(); - zDot.reset(); + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + lastPosition.set(currentPosition); + setToZero(); + } + + currentRawDerivative.sub(currentPosition, lastPosition); + currentRawDerivative.scale(1.0 / dt); + + double alpha = alphaProvider.getValue(); + interpolate(currentRawDerivative, this, alpha); + + lastPosition.set(currentPosition); } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java index 92638f36..250f1faa 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java @@ -2,90 +2,125 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFramePoint3D; -import us.ihmc.yoVariables.variable.YoFrameTuple2D; +import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoFrameVector2D; public class FilteredVelocityYoFrameVector2d extends YoFrameVector2D { - private final FilteredVelocityYoVariable xDot, yDot; - - public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, YoDouble alpha, double dt, - YoVariableRegistry registry, YoFrameTuple2D yoFrameVectorToDifferentiate) + private final double dt; + private final DoubleProvider alphaProvider; + + private final YoBoolean hasBeenCalled; + private final FrameTuple2DReadOnly currentPosition; + private final YoFrameVector2D lastPosition; + + /** + * @deprecated Use + * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoVariableRegistry, FrameTuple2DReadOnly)} + * instead. + */ + @Deprecated + public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, + YoVariableRegistry registry, + FrameTuple2DReadOnly frameTuple2DToDifferentiate) { - return createFilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, "", alpha, dt, registry, yoFrameVectorToDifferentiate); + return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, frameTuple2DToDifferentiate); } - public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, String description, YoDouble alpha, - double dt, YoVariableRegistry registry, - YoFrameTuple2D yoFrameVectorToDifferentiate) + /** + * @deprecated Use + * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoVariableRegistry, FrameTuple2DReadOnly)} + * instead. + */ + @Deprecated + public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, String description, + DoubleProvider alpha, double dt, YoVariableRegistry registry, + FrameTuple2DReadOnly frameTuple2DToDifferentiate) { - FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, alpha, - yoFrameVectorToDifferentiate.getYoX(), dt, registry); - FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, alpha, - yoFrameVectorToDifferentiate.getYoY(), dt, registry); - - FilteredVelocityYoFrameVector2d ret = new FilteredVelocityYoFrameVector2d(xDot, yDot, alpha, dt, registry, - yoFrameVectorToDifferentiate.getReferenceFrame()); - - return ret; + return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, frameTuple2DToDifferentiate); } - public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, YoDouble alpha, double dt, + /** + * @deprecated Use + * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoVariableRegistry, ReferenceFrame)} + * instead. + */ + @Deprecated + public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, ReferenceFrame referenceFrame) { - FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, dt, registry); - FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, dt, registry); - - FilteredVelocityYoFrameVector2d ret = new FilteredVelocityYoFrameVector2d(xDot, yDot, alpha, dt, registry, referenceFrame); - - return ret; + return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, referenceFrame); } - private FilteredVelocityYoFrameVector2d(FilteredVelocityYoVariable xDot, FilteredVelocityYoVariable yDot, YoDouble alpha, double dt, - YoVariableRegistry registry, ReferenceFrame referenceFrame) + public FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, + FrameTuple2DReadOnly frameTuple2DToDifferentiate) { - super(xDot, yDot, referenceFrame); - - this.xDot = xDot; - this.yDot = yDot; + super(namePrefix, nameSuffix, frameTuple2DToDifferentiate.getReferenceFrame(), registry); + this.alphaProvider = alpha; + this.dt = dt; + + hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); + currentPosition = frameTuple2DToDifferentiate; + lastPosition = new YoFrameVector2D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); + reset(); } - private FilteredVelocityYoFrameVector2d(FilteredVelocityYoVariable xDot, FilteredVelocityYoVariable yDot, YoDouble alpha, double dt, - YoVariableRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) + public FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, + ReferenceFrame referenceFrame) { - super(xDot, yDot, yoFramePointToDifferentiate.getReferenceFrame()); + super(namePrefix, nameSuffix, referenceFrame, registry); - this.xDot = xDot; - this.yDot = yDot; + this.alphaProvider = alpha; + this.dt = dt; + + hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); + currentPosition = null; + lastPosition = new YoFrameVector2D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); + reset(); } - public void update() + public void reset() { - xDot.update(); - yDot.update(); + hasBeenCalled.set(false); } - public void update(Vector2DReadOnly vector) + public void update() { - xDot.update(vector.getX()); - yDot.update(vector.getY()); + if (currentPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(FrameTuple3DReadOnly)"); + } + + update(currentPosition); } - public void update(FrameTuple2DReadOnly tupleToDifferentiate) + public void update(FrameTuple2DReadOnly frameTuple) { - checkReferenceFrameMatch(tupleToDifferentiate); - xDot.update(tupleToDifferentiate.getX()); - yDot.update(tupleToDifferentiate.getY()); + checkReferenceFrameMatch(frameTuple); + update((Tuple2DReadOnly) frameTuple); } - public void reset() + private final Vector2D currentRawDerivative = new Vector2D(); + + public void update(Tuple2DReadOnly currentPosition) { - xDot.reset(); - yDot.reset(); + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + lastPosition.set(currentPosition); + setToZero(); + } + + currentRawDerivative.sub(currentPosition, lastPosition); + currentRawDerivative.scale(1.0 / dt); + + interpolate(currentRawDerivative, this, alphaProvider.getValue()); + + lastPosition.set(currentPosition); } } From 3072938c1b79b7c7e04f1981a5037ab6a2daae66 Mon Sep 17 00:00:00 2001 From: Jesper Smith Date: Tue, 28 Aug 2018 11:28:36 +0200 Subject: [PATCH 11/47] Added some easier constructors for AlhpaBasedOnBreakFrequencyProvider --- .../AlphaBasedOnBreakFrequencyProvider.java | 42 +++++++++++++++++-- 1 file changed, 39 insertions(+), 3 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java index a0850252..1e0b1885 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java @@ -1,13 +1,15 @@ package us.ihmc.robotics.math.filters; +import us.ihmc.yoVariables.parameters.DoubleParameter; import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; /** * This class calculates the alpha value given a break frequency provider * - * The value gets cached to avoid unneccessary calculations + * The value gets cached to avoid unnecessary calculations * - * @author jesper + * @author Jesper Smith * */ public class AlphaBasedOnBreakFrequencyProvider implements DoubleProvider @@ -19,20 +21,54 @@ public class AlphaBasedOnBreakFrequencyProvider implements DoubleProvider private double previousBreakFrequency = Double.NaN; private double alpha = 0.0; + /** + * Create a new provider using the break frequency provided by double provider + * + * @param breakFrequencyProvider + * @param dt + */ public AlphaBasedOnBreakFrequencyProvider(DoubleProvider breakFrequencyProvider, double dt) { this.breakFrequencyProvider = breakFrequencyProvider; this.dt = dt; } - @Override + + /** + * Create a new provider backed by a frequency with the given name. + * + * @param name Name of the break frequency parameter + * @param dt Time step + * @param registry Parent registry for the break frequency parameter + * @param defaultBreakFrequency Default value for the break frequency + */ + public AlphaBasedOnBreakFrequencyProvider(String name, double dt, YoVariableRegistry registry, double defaultBreakFrequency) + { + this(new DoubleParameter(name, registry, defaultBreakFrequency), dt); + } + + /** + * Create a new provider backed by a frequency with the given name. + * + * @param name Name of the break frequency parameter + * @param dt Time step + * @param registry Parent registry for the break frequency parameter + */ + public AlphaBasedOnBreakFrequencyProvider(String name, double dt, YoVariableRegistry registry) + { + this.breakFrequencyProvider = new DoubleParameter(name, registry, Double.NaN); + this.dt = dt; + } + /** * 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(); From ce0587b0732fd4deadc6774a70fb53297866d26b Mon Sep 17 00:00:00 2001 From: Georg Wiedebach Date: Wed, 15 Aug 2018 19:54:16 -0700 Subject: [PATCH 12/47] Add a filter to the precomputed ICP trajectory to get smoother desireds. --- .../math/filters/AlphaFilteredTuple2D.java | 97 +++++++++++++++++++ 1 file changed, 97 insertions(+) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java new file mode 100644 index 00000000..79f84837 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java @@ -0,0 +1,97 @@ +package us.ihmc.robotics.math.filters; + +import org.apache.commons.lang3.NotImplementedException; + +import us.ihmc.euclid.transform.interfaces.Transform; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; +import us.ihmc.yoVariables.providers.DoubleProvider; + +public class AlphaFilteredTuple2D implements Tuple2DBasics +{ + private final DoubleProvider alpha; + + private boolean resetX; + private boolean resetY; + + private double x; + private double y; + + public AlphaFilteredTuple2D(DoubleProvider alpha) + { + this.alpha = alpha; + reset(); + } + + public AlphaFilteredTuple2D(Tuple2DReadOnly other, DoubleProvider alpha) + { + this.alpha = alpha; + reset(other); + } + + public void reset() + { + resetX = true; + resetY = true; + } + + public void reset(Tuple2DReadOnly other) + { + resetX = true; + resetY = true; + set(other); + } + + @Override + public double getX() + { + return x; + } + + @Override + public double getY() + { + return y; + } + + @Override + public void setX(double x) + { + if (resetX) + { + this.x = x; + resetX = false; + } + else + { + this.x = alpha.getValue() * this.x + (1.0 - alpha.getValue()) * x; + } + } + + @Override + public void setY(double y) + { + if (resetY) + { + this.y = y; + resetY = false; + } + else + { + this.y = alpha.getValue() * this.y + (1.0 - alpha.getValue()) * y; + } + } + + @Override + public void applyTransform(Transform transform, boolean checkIfTransformInXYPlane) + { + throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + "."); + } + + @Override + public void applyInverseTransform(Transform transform, boolean checkIfTransformInXYPlane) + { + throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + "."); + } + +} From bc82f623f5bde397b4eae002610e1e4d6101053b Mon Sep 17 00:00:00 2001 From: Georg Wiedebach Date: Tue, 11 Sep 2018 11:38:57 -0700 Subject: [PATCH 13/47] Parameterize lowest foot option and add a glitch filter to the lowest foot to avoid bouncing if the feet are at the same height. --- .../math/filters/GlitchFilteredYoInteger.java | 37 ++++++++++++++++--- 1 file changed, 31 insertions(+), 6 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java index 92fa4f63..eed1c217 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java @@ -1,5 +1,6 @@ package us.ihmc.robotics.math.filters; +import us.ihmc.yoVariables.providers.IntegerProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoInteger; @@ -7,7 +8,7 @@ public class GlitchFilteredYoInteger extends YoInteger { private final YoInteger position; private final YoInteger previousPosition; - private final YoInteger windowSize; + private final IntegerProvider windowSize; private final YoInteger counter; public GlitchFilteredYoInteger(String name, int windowSize, YoVariableRegistry registry) @@ -23,8 +24,25 @@ public GlitchFilteredYoInteger(String name, int windowSize, YoInteger position, previousPosition = new YoInteger(name + "PrevValue", registry); counter = new YoInteger(name + "Count", registry); - this.windowSize = new YoInteger(name + "WindowSize", registry); - this.windowSize.set(windowSize); + YoInteger yoWindowSize = new YoInteger(name + "WindowSize", registry); + yoWindowSize.set(windowSize); + this.windowSize = yoWindowSize; + } + + public GlitchFilteredYoInteger(String name, IntegerProvider windowSize, YoVariableRegistry registry) + { + this(name, windowSize, null, registry); + } + + public GlitchFilteredYoInteger(String name, IntegerProvider windowSize, YoInteger position, YoVariableRegistry registry) + { + super(name, GlitchFilteredYoInteger.class.getSimpleName(), registry); + + this.position = position; + + previousPosition = new YoInteger(name + "PrevValue", registry); + counter = new YoInteger(name + "Count", registry); + this.windowSize = windowSize; } @Override @@ -61,7 +79,7 @@ public void update(int currentValue) else counter.set(0); - if (counter.getIntegerValue() >= windowSize.getIntegerValue()) + if (counter.getIntegerValue() >= windowSize.getValue()) { set(currentValue); counter.set(0); @@ -72,11 +90,18 @@ public void update(int currentValue) public int getWindowSize() { - return windowSize.getIntegerValue(); + return windowSize.getValue(); } public void setWindowSize(int windowSize) { - this.windowSize.set(windowSize); + if (this.windowSize instanceof YoInteger) + { + ((YoInteger) this.windowSize).set(windowSize); + } + else + { + throw new RuntimeException("Setting the window size is not supported"); + } } } From 64c8a1c6ad552a1586673927b7dde0ce3a4dfbdf Mon Sep 17 00:00:00 2001 From: AwareOperator Date: Wed, 12 Sep 2018 16:53:09 -0700 Subject: [PATCH 14/47] started setting up the rate limiting of the contact point locations during toe off create a set and update flag for the rate limited yo frame point 2d Forgot to create mutable frame rate limited vectors. Fixed compilation errors. --- ...ifferenceAngularVelocityYoFrameVector.java | 15 +- .../RateLimitedYoFrameOrientation.java | 5 +- .../filters/RateLimitedYoFramePoint2D.java | 136 ++++++++++++++++++ .../filters/RateLimitedYoFrameQuaternion.java | 15 +- .../RateLimitedYoMutableFrameVector3D.java | 121 ++++++++++++++++ 5 files changed, 276 insertions(+), 16 deletions(-) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java index 528ed7e2..18067e7f 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java @@ -1,12 +1,11 @@ package us.ihmc.robotics.math.filters; import us.ihmc.euclid.axisAngle.AxisAngle; -import us.ihmc.euclid.axisAngle.interfaces.AxisAngleReadOnly; import us.ihmc.euclid.matrix.RotationMatrix; import us.ihmc.euclid.matrix.interfaces.RotationMatrixReadOnly; +import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoFrameQuaternion; @@ -61,7 +60,7 @@ public void update() update(currentOrientationMatrix); } - public void update(FrameQuaternionReadOnly currentOrientation) + public void update(FrameOrientation3DReadOnly currentOrientation) { checkReferenceFrameMatch(currentOrientation); @@ -69,13 +68,7 @@ public void update(FrameQuaternionReadOnly currentOrientation) update(currentOrientationMatrix); } - public void update(QuaternionReadOnly currentOrientation) - { - currentOrientationMatrix.set(currentOrientation); - update(currentOrientationMatrix); - } - - public void update(AxisAngleReadOnly currentOrientation) + public void update(Orientation3DReadOnly currentOrientation) { currentOrientationMatrix.set(currentOrientation); update(currentOrientationMatrix); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java index 1e86932d..d7ccddce 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java @@ -91,8 +91,7 @@ public void update() public void update(YoFrameYawPitchRoll yoFrameVectorUnfiltered) { checkReferenceFrameMatch(yoFrameVectorUnfiltered); - update(yoFrameVectorUnfiltered.getYaw().getDoubleValue(), yoFrameVectorUnfiltered.getPitch().getDoubleValue(), - yoFrameVectorUnfiltered.getRoll().getDoubleValue()); + update(yoFrameVectorUnfiltered.getYaw(), yoFrameVectorUnfiltered.getPitch(), yoFrameVectorUnfiltered.getRoll()); } public void update(FrameQuaternion frameOrientationUnfiltered) @@ -123,7 +122,7 @@ public void update(QuaternionReadOnly quaternionUnfiltered) return; } - quaternionFiltered.set(getFrameOrientation()); + quaternionFiltered.set(this); if (quaternionFiltered.dot(quaternionUnfiltered) > 0.0) { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java new file mode 100644 index 00000000..109ea90f --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java @@ -0,0 +1,136 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFramePoint2D; + +public class RateLimitedYoFramePoint2D extends YoFramePoint2D +{ + private final DoubleProvider maxRateVariable; + + private final FrameTuple2DReadOnly rawPosition; + private final YoBoolean limited; + private final YoBoolean hasBeenCalled; + private final double dt; + + private final FrameVector2D differenceVector = new FrameVector2D(); + + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } + + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple2DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); + } + + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); + } + + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + FrameTuple2DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, + rawPosition.getReferenceFrame()); + } + + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); + } + + private RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple2DReadOnly rawPosition, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); + this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); + + if (maxRate == null) + maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; + + this.rawPosition = rawPosition; + + this.dt = dt; + + reset(); + } + + public void setAndUpdate(FramePoint2DReadOnly framePoint2D) + { + super.set(framePoint2D); + hasBeenCalled.set(true); + } + + public void setAndUpdate(FramePoint3DReadOnly framePoint3D) + { + super.set(framePoint3D); + hasBeenCalled.set(true); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawPosition); + } + + public void update(FrameTuple2DReadOnly frameVectorUnfiltered) + { + checkReferenceFrameMatch(frameVectorUnfiltered); + update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY()); + } + + public void update(Tuple2DReadOnly vectorUnfiltered) + { + update(vectorUnfiltered.getX(), vectorUnfiltered.getY()); + } + + public void update(double xUnfiltered, double yUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered); + } + + if (maxRateVariable.getValue() < 0) + throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); + + differenceVector.setToZero(getReferenceFrame()); + differenceVector.set(xUnfiltered, yUnfiltered); + differenceVector.sub(getX(), getY()); + + limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); + add(differenceVector); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java index 6803d04d..de863547 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java @@ -1,6 +1,7 @@ package us.ihmc.robotics.math.filters; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple4D.Quaternion; @@ -94,10 +95,20 @@ public void update() update(rawQuaternion); } - public void update(FrameQuaternionReadOnly frameOrientationUnfiltered) + private final Quaternion quaternion = new Quaternion(); + + public void update(FrameOrientation3DReadOnly frameOrientationUnfiltered) { checkReferenceFrameMatch(frameOrientationUnfiltered); - update((QuaternionReadOnly) frameOrientationUnfiltered); + quaternion.set(frameOrientationUnfiltered); + update(quaternion); + } + + public void update(FrameQuaternionReadOnly frameQuaternionUnfiltered) + { + checkReferenceFrameMatch(frameQuaternionUnfiltered); + quaternion.set(frameQuaternionUnfiltered); + update(quaternion); } private final Quaternion difference = new Quaternion(); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java new file mode 100644 index 00000000..3c794ee6 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java @@ -0,0 +1,121 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.frameObjects.YoMutableFrameVector3D; + +public class RateLimitedYoMutableFrameVector3D extends YoMutableFrameVector3D +{ + private final DoubleProvider maxRateVariable; + + private final FrameTuple3DReadOnly rawPosition; + private final YoBoolean limited; + private final YoBoolean hasBeenCalled; + private final double dt; + + private final FrameVector3D differenceVector = new FrameVector3D(); + + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } + + public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); + } + + public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); + } + + public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + FrameTuple3DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, + rawPosition.getReferenceFrame()); + } + + public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); + } + + private RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, registry, referenceFrame); + + this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); + this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); + + if (maxRate == null) + maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; + + this.rawPosition = rawPosition; + + this.dt = dt; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawPosition); + } + + public void update(FrameTuple3DReadOnly frameVectorUnfiltered) + { + checkReferenceFrameMatch(frameVectorUnfiltered); + update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); + } + + public void update(Tuple3DReadOnly vectorUnfiltered) + { + update(vectorUnfiltered.getX(), vectorUnfiltered.getY(), vectorUnfiltered.getZ()); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered, zUnfiltered); + } + + if (maxRateVariable.getValue() < 0) + throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); + + differenceVector.setToZero(getReferenceFrame()); + differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); + differenceVector.sub(getX(), getY(), getZ()); + + limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); + add(differenceVector); + } +} From 068a0116589f0ef66ba48e271d41018e46955272 Mon Sep 17 00:00:00 2001 From: Robert Date: Sun, 19 May 2019 17:05:15 -0500 Subject: [PATCH 15/47] created an acceleration limited yo frame vector 3d and updated the existing acceleration limited yo variable to use double providers completely overhualed the acceleration limited yo frame vector 3D --- .../AccelerationLimitedYoFrameVector2d.java | 5 +- .../AccelerationLimitedYoFrameVector3D.java | 173 ++++++++++++++++++ .../AccelerationLimitedYoVariable.java | 43 ++--- 3 files changed, 188 insertions(+), 33 deletions(-) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java index dc06b481..bbcc0f3d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java @@ -5,6 +5,7 @@ import us.ihmc.euclid.referenceFrame.FrameVector2D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoFrameVector2D; @@ -23,7 +24,7 @@ private AccelerationLimitedYoFrameVector2d(AccelerationLimitedYoVariable x, Acce } public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, - YoDouble maxRate, YoDouble maxAcceleration, double dt, ReferenceFrame referenceFrame) + DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt, ReferenceFrame referenceFrame) { AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); @@ -35,7 +36,7 @@ public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFram public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, - YoDouble maxRate, YoDouble maxAcceleration, double dt, YoFrameVector2D unfilteredVector) + DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt, YoFrameVector2D unfilteredVector) { AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoX(), dt); AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoY(), dt); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java new file mode 100644 index 00000000..00675248 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java @@ -0,0 +1,173 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoFrameVector3D; + +public class AccelerationLimitedYoFrameVector3D extends YoFrameVector3D +{ + private final DoubleProvider maxRateVariable; + private final DoubleProvider maxAccelerationVariable; + + private final FrameTuple3DReadOnly rawPosition; + private final YoBoolean accelerationLimited; + private final YoBoolean rateLimited; + + private final YoBoolean hasBeenInitialized; + private final YoDouble positionGain; + private final YoDouble velocityGain; + + private final YoFrameVector3D smoothedRate; + private final YoFrameVector3D smoothedAcceleration; + + private final double dt; + + private final FrameVector3D positionError = new FrameVector3D(); + private final FrameVector3D acceleration = new FrameVector3D(); + + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } + + private static DoubleProvider createMaxAccelerationYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxAcceleration" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } + + public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, + DoubleProvider maxAcceleration, double dt, FrameTuple3DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, maxRate, maxAcceleration, dt, rawPosition, rawPosition.getReferenceFrame()); + } + + public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, + DoubleProvider maxAcceleration, double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, maxAcceleration, dt, null, referenceFrame); + } + public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double maxAcceleration, + double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), + createMaxAccelerationYoDouble(namePrefix, nameSuffix, maxAcceleration, registry), dt, null, referenceFrame); + } + + private AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, + DoubleProvider maxAcceleration, double dt, FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + if (maxRate == null) + maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + if (maxAcceleration == null) + maxAcceleration = createMaxAccelerationYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + this.maxRateVariable = maxRate; + this.maxAccelerationVariable = maxAcceleration; + + this.dt = dt; + + hasBeenInitialized = new YoBoolean(namePrefix + "HasBeenInitialized" + namePrefix, registry); + this.rateLimited = new YoBoolean(namePrefix + "RateLimited" + nameSuffix, registry); + this.accelerationLimited = new YoBoolean(namePrefix + "AccelerationLimited" + nameSuffix, registry); + + smoothedRate = new YoFrameVector3D(namePrefix + "SmoothedRate" + namePrefix, referenceFrame, registry); + smoothedAcceleration = new YoFrameVector3D(namePrefix + "SmoothedAcceleration" + namePrefix, referenceFrame, registry); + + positionGain = new YoDouble(namePrefix + "PositionGain" + namePrefix, registry); + velocityGain = new YoDouble(namePrefix + "VelocityGain" + namePrefix, registry); + + double w0 = 2.0 * Math.PI * 16.0; + double zeta = 1.0; + + setGainsByPolePlacement(w0, zeta); + hasBeenInitialized.set(false); + + this.rawPosition = rawPosition; + + } + + + public void setGainsByPolePlacement(double w0, double zeta) + { + positionGain.set(w0 * w0); + velocityGain.set(2.0 * zeta * w0); + } + + public void update() + { + if (rawPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawPosition); + } + + public void update(FrameTuple3DReadOnly frameVectorUnfiltered) + { + checkReferenceFrameMatch(frameVectorUnfiltered); + update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + if (!hasBeenInitialized.getBooleanValue()) + initialize(xUnfiltered, yUnfiltered, zUnfiltered); + + positionError.set(xUnfiltered, yUnfiltered, zUnfiltered); + positionError.sub(this); + + acceleration.set(smoothedRate); + acceleration.scale(-velocityGain.getValue()); + acceleration.scaleAdd(positionGain.getValue(), positionError, acceleration); + + accelerationLimited.set(acceleration.clipToMaxLength(maxAccelerationVariable.getValue())); + + smoothedAcceleration.set(acceleration); + smoothedRate.scaleAdd(dt, smoothedAcceleration, smoothedRate); + + rateLimited.set(smoothedRate.clipToMaxLength(maxRateVariable.getValue())); + + this.scaleAdd(dt, smoothedRate, this); + + if (this.containsNaN()) + throw new RuntimeException("what?"); + + } + + public void initialize(FrameTuple3DReadOnly input) + { + initialize(input.getX(), input.getY(), input.getZ()); + } + + public void initialize(double xInput, double yInput, double zInput) + { + this.set(xInput, yInput, zInput); + smoothedRate.setToZero(); + smoothedAcceleration.setToZero(); + + this.hasBeenInitialized.set(true); + } + + public void reset() + { + this.hasBeenInitialized.set(false); + smoothedRate.setToZero(); + smoothedAcceleration.setToZero(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java index 971a4e27..611acd53 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java @@ -1,6 +1,7 @@ package us.ihmc.robotics.math.filters; import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -17,29 +18,18 @@ public class AccelerationLimitedYoVariable extends YoDouble private final YoDouble positionGain; private final YoDouble velocityGain; - private YoDouble maximumRate; - private YoDouble maximumAcceleration; + private DoubleProvider maximumRate; + private DoubleProvider maximumAcceleration; - private final YoDouble inputVariable; + private final DoubleProvider inputVariable; - public AccelerationLimitedYoVariable(String name, YoVariableRegistry registry, double maxRate, double maxAcceleration, YoDouble inputVariable, double dt) - { - this(name, registry, null, null, inputVariable, dt); - - maximumRate = new YoDouble(name + "MaxRate", registry); - maximumAcceleration = new YoDouble(name + "MaxAcceleration", registry); - - maximumRate.set(maxRate); - maximumAcceleration.set(maxAcceleration); - } - - public AccelerationLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxRate, YoDouble maxAcceleration, double dt) + public AccelerationLimitedYoVariable(String name, YoVariableRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt) { this(name, registry, maxRate, maxAcceleration, null, dt); } - public AccelerationLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxRate, YoDouble maxAcceleration, - YoDouble inputVariable, double dt) + public AccelerationLimitedYoVariable(String name, YoVariableRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, + DoubleProvider inputVariable, double dt) { super(name, registry); @@ -68,15 +58,6 @@ public AccelerationLimitedYoVariable(String name, YoVariableRegistry registry, Y this.inputVariable = inputVariable; } - public void setMaximumAcceleration(double maximumAcceleration) - { - this.maximumAcceleration.set(maximumAcceleration); - } - - public void setMaximumRate(double maximumRate) - { - this.maximumRate.set(maximumRate); - } public void setGainsByPolePlacement(double w0, double zeta) { @@ -96,7 +77,7 @@ public YoDouble getVelocityGain() public void update() { - update(inputVariable.getDoubleValue()); + update(inputVariable.getValue()); } public void update(double input) @@ -106,11 +87,11 @@ public void update(double input) double positionError = input - this.getDoubleValue(); double acceleration = -velocityGain.getDoubleValue() * smoothedRate.getDoubleValue() + positionGain.getDoubleValue() * positionError; - acceleration = MathTools.clamp(acceleration, -maximumAcceleration.getDoubleValue(), maximumAcceleration.getDoubleValue()); + acceleration = MathTools.clamp(acceleration, -maximumAcceleration.getValue(), maximumAcceleration.getValue()); smoothedAcceleration.set(acceleration); smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); - smoothedRate.set(MathTools.clamp(smoothedRate.getDoubleValue(), maximumRate.getDoubleValue())); + smoothedRate.set(MathTools.clamp(smoothedRate.getDoubleValue(), maximumRate.getValue())); this.add(smoothedRate.getDoubleValue() * dt); } @@ -147,11 +128,11 @@ public boolean hasBeenInitialized() public double getMaximumRate() { - return maximumRate.getDoubleValue(); + return maximumRate.getValue(); } public double getMaximumAcceleration() { - return maximumAcceleration.getDoubleValue(); + return maximumAcceleration.getValue(); } } \ No newline at end of file From b70ef712a0e40b7d280d86be1a9a188e708019e4 Mon Sep 17 00:00:00 2001 From: Georg Wiedebach Date: Thu, 27 Jun 2019 10:38:11 -0700 Subject: [PATCH 16/47] Added reset method to processing YoVariable since some of them can have internal state. --- .../robotics/math/filters/AlphaFilteredYoFrameQuaternion.java | 1 + .../robotics/math/filters/AlphaFilteredYoFrameVector.java | 1 + .../ihmc/robotics/math/filters/AlphaFilteredYoVariable.java | 1 + .../math/filters/BacklashCompensatingVelocityYoVariable.java | 2 ++ .../robotics/math/filters/BacklashProcessingYoVariable.java | 1 + .../robotics/math/filters/FilteredVelocityYoVariable.java | 1 + .../us/ihmc/robotics/math/filters/ProcessingYoVariable.java | 4 ++++ .../RevisedBacklashCompensatingVelocityYoVariable.java | 1 + .../math/filters/SecondOrderFilteredYoFrameVector.java | 4 +++- .../filters/SimpleMovingAverageFilteredYoFrameVector.java | 2 ++ 10 files changed, 17 insertions(+), 1 deletion(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java index 76d4defa..79da3dc1 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java @@ -84,6 +84,7 @@ public void update(Quaternion qMeasured) } } + @Override public void reset() { hasBeenCalled.set(false); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java index 0c0fcd2c..1ef2baf5 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java @@ -99,6 +99,7 @@ private AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVaria reset(); } + @Override public void reset() { hasBeenCalled.set(false); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java index 8e9aeac9..501b0e38 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java @@ -88,6 +88,7 @@ public AlphaFilteredYoVariable(String name, String description, YoVariableRegist reset(); } + @Override public void reset() { hasBeenCalled.set(false); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java index 5203f46d..356c93fa 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java @@ -65,12 +65,14 @@ public BacklashCompensatingVelocityYoVariable(String name, String description, Y reset(); } + @Override public void reset() { hasBeenCalled.set(false); backlashState.set(null); } + @Override public void update() { if (position == null) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java index a5487305..63165b6c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java @@ -49,6 +49,7 @@ public BacklashProcessingYoVariable(String name, String description, YoDouble ve reset(); } + @Override public void reset() { hasBeenCalled.set(false); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java index 8e835aa5..444c7710 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java @@ -98,6 +98,7 @@ public FilteredVelocityYoVariable(String name, String description, DoubleProvide reset(); } + @Override public void reset() { hasBeenCalled.set(false); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java index 7353f617..14b60182 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java @@ -3,4 +3,8 @@ public interface ProcessingYoVariable { public abstract void update(); + + public default void reset() + { + } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java index 0ab38834..40f7c194 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java @@ -81,6 +81,7 @@ public RevisedBacklashCompensatingVelocityYoVariable(String name, String descrip reset(); } + @Override public void reset() { hasBeenCalled.set(false); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java index 51bb8535..ebdf4d31 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java @@ -3,9 +3,9 @@ import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoFrameVector3D; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; public class SecondOrderFilteredYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable { @@ -57,6 +57,7 @@ public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameV return new SecondOrderFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); } + @Override public void update() { x.update(); @@ -86,6 +87,7 @@ public void update(FrameVector3D vectorUnfiltered) z.update(vectorUnfiltered.getZ()); } + @Override public void reset() { x.reset(); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java index f09e918e..087ad981 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java @@ -52,6 +52,7 @@ public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverage return new SimpleMovingAverageFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); } + @Override public void update() { x.update(); @@ -81,6 +82,7 @@ public void update(FrameVector3D vectorUnfiltered) z.update(vectorUnfiltered.getZ()); } + @Override public void reset() { x.reset(); From 37422df86410594b976177766a8d403ae5550c76 Mon Sep 17 00:00:00 2001 From: Valkyrie Date: Fri, 11 Oct 2019 17:04:37 -0500 Subject: [PATCH 17/47] Made the API more flexible. --- .../AlphaFilteredYoFrameQuaternion.java | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java index 79da3dc1..b4d20031 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java @@ -1,7 +1,10 @@ package us.ihmc.robotics.math.filters; +import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoBoolean; @@ -68,7 +71,20 @@ public void update() update(qMeasured); } - public void update(Quaternion qMeasured) + public void update(FrameOrientation3DReadOnly rawOrientation) + { + checkReferenceFrameMatch(rawOrientation); + qMeasured.set(rawOrientation); + update(qMeasured); + } + + public void update(Orientation3DReadOnly rawOrientation) + { + qMeasured.set(rawOrientation); + update(qMeasured); + } + + private void update(QuaternionReadOnly qMeasured) { if (hasBeenCalled.getBooleanValue()) { From 5e65ed393d32bfa8fa256e27df0e958bcf0f071f Mon Sep 17 00:00:00 2001 From: OperatorComputer Date: Wed, 19 Feb 2020 08:28:09 -0600 Subject: [PATCH 18/47] TypeCast as double just to make sure. --- .../robotics/math/filters/RateLimitedYoFrameVector2d.java | 3 ++- .../filters/SimpleMovingAverageFilteredYoVariable.java | 8 ++------ 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java index 3a04be0d..4afeb977 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java @@ -5,6 +5,7 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoFrameVector2D; @@ -58,7 +59,7 @@ public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, - YoDouble maxRate, double dt, YoFrameVector2D unfilteredVector) + DoubleProvider maxRate, double dt, YoFrameVector2D unfilteredVector) { RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java index b4134e6b..776230c3 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java @@ -8,11 +8,7 @@ import us.ihmc.yoVariables.variable.YoInteger; /** - * Filter the given yoVariable using a moving average filter. - * - * - * This class is NOT REWINDABLE! - * + * Filter the given yoVariable using a moving average filter. This class is NOT REWINDABLE! */ public class SimpleMovingAverageFilteredYoVariable extends YoDouble { @@ -74,7 +70,7 @@ public void update(double value) average += previousUpdateValues.get(i, 0); } - this.set(average / windowSize.getIntegerValue()); + this.set(average / ((double) windowSize.getIntegerValue())); } public void reset() From e6a7a6a709330ba5521248ab39e150a695672bee Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Tue, 9 Jun 2020 12:43:57 -0500 Subject: [PATCH 19/47] :arrow_up: EJML, GeoRegression, DDogleg, BoofCV. --- .../filters/SimpleMovingAverageFilteredYoVariable.java | 8 ++++---- .../main/java/us/ihmc/robotics/math/frames/YoMatrix.java | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java index 776230c3..17d7cfef 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java @@ -1,7 +1,7 @@ package us.ihmc.robotics.math.filters; -import org.ejml.data.DenseMatrix64F; -import org.ejml.ops.CommonOps; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoDouble; @@ -15,7 +15,7 @@ public class SimpleMovingAverageFilteredYoVariable extends YoDouble private final YoInteger windowSize; private final YoDouble yoVariableToFilter; - private final DenseMatrix64F previousUpdateValues = new DenseMatrix64F(0, 0); + private final DMatrixRMaj previousUpdateValues = new DMatrixRMaj(0, 0); private int bufferPosition = 0; private boolean bufferHasBeenFilled = false; @@ -34,7 +34,7 @@ public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoDoub this.windowSize.set(windowSize); previousUpdateValues.reshape(windowSize, 1); - CommonOps.fill(previousUpdateValues, 0.0); + CommonOps_DDRM.fill(previousUpdateValues, 0.0); } public void setWindowSize(int windowSize) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java index 253c3fa3..a85a4e38 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.frames; -import org.ejml.data.DenseMatrix64F; +import org.ejml.data.DMatrixRMaj; import us.ihmc.yoVariables.registry.YoVariableRegistry; import us.ihmc.yoVariables.variable.YoDouble; @@ -45,7 +45,7 @@ public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, YoVari } } - public void set(DenseMatrix64F matrix) + public void set(DMatrixRMaj matrix) { int numRows = matrix.getNumRows(); int numCols = matrix.getNumCols(); @@ -72,13 +72,13 @@ public void set(DenseMatrix64F matrix) } } - public void getAndReshape(DenseMatrix64F matrixToPack) + public void getAndReshape(DMatrixRMaj matrixToPack) { matrixToPack.reshape(getNumberOfRows(), getNumberOfColumns()); get(matrixToPack); } - public void get(DenseMatrix64F matrixToPack) + public void get(DMatrixRMaj matrixToPack) { int numRows = matrixToPack.getNumRows(); int numCols = matrixToPack.getNumCols(); From 919f06b84e89c02505269fbed7197c2f4e9ff6a9 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Fri, 26 Jun 2020 11:32:51 -0500 Subject: [PATCH 20/47] Added some YoVariables. --- .../ihmc/robotics/math/frames/YoMatrix.java | 28 ++++++++++++++----- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java index a85a4e38..9d9e08b8 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java @@ -7,14 +7,12 @@ import us.ihmc.yoVariables.variable.YoInteger; /** - * YoMatrix. Object for holding a matrix of YoVariables so that Matrices can be rewound. - * Has a maximum number of rows and columns and an actual number of rows and columns. - * If you set with a smaller matrix, then the actual size will be the size of the - * passed in matrix. extra - * entries will be set to NaN. If you get the contents the matrix you pack must be the - * correct size. + * YoMatrix. Object for holding a matrix of YoVariables so that Matrices can be rewound. Has a + * maximum number of rows and columns and an actual number of rows and columns. If you set with a + * smaller matrix, then the actual size will be the size of the passed in matrix. extra entries will + * be set to NaN. If you get the contents the matrix you pack must be the correct size. + * * @author JerryPratt - * */ public class YoMatrix { @@ -129,6 +127,22 @@ public void setToZero(int numberOfRows, int numberOfColumns) } } } + } + + public void setToNaN(int numberOfRows, int numberOfColumns) + { + if (((numberOfRows > maxNumberOfRows) || (numberOfColumns > maxNumberOfColumns)) && (numberOfRows > 0) && (numberOfColumns > 0)) + throw new RuntimeException("Not enough rows or columns: " + numberOfRows + " by " + numberOfColumns); + + this.numberOfRows.set(numberOfRows); + this.numberOfColumns.set(numberOfColumns); + for (int row = 0; row < maxNumberOfRows; row++) + { + for (int column = 0; column < maxNumberOfColumns; column++) + { + variables[row][column].set(Double.NaN); + } + } } } From 9c1c1e09bb90508bd0c069701d8e5c42b7337a39 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Sat, 18 Jul 2020 19:24:53 -0500 Subject: [PATCH 21/47] Applied cleanup. Started cleanup and documentation of the tools. Fix frame problem in AccelerationLimitedYoFrameVector3D --- .../AccelerationLimitedYoFrameVector2d.java | 19 +++++----- .../AccelerationLimitedYoFrameVector3D.java | 26 ++++++------- .../AccelerationLimitedYoVariable.java | 6 +-- .../AlphaBasedOnBreakFrequencyProvider.java | 6 +-- .../filters/AlphaBetaFilteredYoVariable.java | 6 +-- .../AlphaFilteredWrappingYoVariable.java | 4 +- .../filters/AlphaFilteredYoFramePoint.java | 30 +++++++-------- .../filters/AlphaFilteredYoFramePoint2d.java | 34 ++++++++--------- .../AlphaFilteredYoFrameQuaternion.java | 14 +++---- .../filters/AlphaFilteredYoFrameVector.java | 30 +++++++-------- .../filters/AlphaFilteredYoFrameVector2d.java | 38 +++++++++---------- .../math/filters/AlphaFilteredYoVariable.java | 16 ++++---- ...lashCompensatingVelocityYoFrameVector.java | 28 +++++++------- ...acklashCompensatingVelocityYoVariable.java | 6 +-- .../filters/BacklashProcessingYoVariable.java | 6 +-- .../filters/BetaFilteredYoFramePoint2d.java | 24 ++++++------ .../filters/BetaFilteredYoFrameVector2d.java | 18 ++++----- .../math/filters/BetaFilteredYoVariable.java | 10 ++--- .../math/filters/DeadzoneYoFrameVector.java | 28 +++++++------- .../math/filters/DeadzoneYoVariable.java | 8 ++-- .../math/filters/DelayedYoBoolean.java | 4 +- .../math/filters/DelayedYoDouble.java | 4 +- .../math/filters/DeltaLimitedYoVariable.java | 4 +- .../FilteredDiscreteVelocityYoVariable.java | 14 +++---- .../FilteredVelocityYoFrameVector.java | 16 ++++---- .../FilteredVelocityYoFrameVector2d.java | 20 +++++----- .../filters/FilteredVelocityYoVariable.java | 10 ++--- ...ifferenceAngularVelocityYoFrameVector.java | 12 +++--- .../FirstOrderBandPassFilteredYoVariable.java | 6 +-- .../filters/FirstOrderFilteredYoVariable.java | 6 +-- .../math/filters/GlitchFilteredYoBoolean.java | 24 ++++++------ .../math/filters/GlitchFilteredYoInteger.java | 14 +++---- .../math/filters/JerkLimitedYoVariable.java | 10 ++--- .../RateLimitedYoFrameOrientation.java | 16 ++++---- .../math/filters/RateLimitedYoFramePoint.java | 16 ++++---- .../filters/RateLimitedYoFramePoint2D.java | 18 ++++----- .../math/filters/RateLimitedYoFramePose.java | 14 +++---- .../filters/RateLimitedYoFrameQuaternion.java | 18 ++++----- .../filters/RateLimitedYoFrameVector.java | 16 ++++---- .../filters/RateLimitedYoFrameVector2d.java | 30 +++++++-------- .../RateLimitedYoMutableFrameVector3D.java | 16 ++++---- .../math/filters/RateLimitedYoVariable.java | 10 ++--- ...acklashCompensatingVelocityYoVariable.java | 6 +-- .../SecondOrderFilteredYoFrameVector.java | 26 ++++++------- ...condOrderFilteredYoVariableParameters.java | 4 +- ...pleMovingAverageFilteredYoFrameVector.java | 25 ++++++------ ...SimpleMovingAverageFilteredYoVariable.java | 6 +-- .../ihmc/robotics/math/frames/YoMatrix.java | 4 +- 48 files changed, 360 insertions(+), 366 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java index bbcc0f3d..5bbdf29d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java @@ -5,11 +5,10 @@ import us.ihmc.euclid.referenceFrame.FrameVector2D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFrameVector2D; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; public class AccelerationLimitedYoFrameVector2d extends YoFrameVector2D { @@ -23,11 +22,11 @@ private AccelerationLimitedYoFrameVector2d(AccelerationLimitedYoVariable x, Acce this.y = y; } - public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt, ReferenceFrame referenceFrame) { - AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); - AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); + AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); + AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); AccelerationLimitedYoFrameVector2d ret = new AccelerationLimitedYoFrameVector2d(x, y, referenceFrame); @@ -35,11 +34,11 @@ public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFram } - public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt, YoFrameVector2D unfilteredVector) { - AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoX(), dt); - AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoY(), dt); + AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoX(), dt); + AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoY(), dt); AccelerationLimitedYoFrameVector2d ret = new AccelerationLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java index 00675248..2873da53 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java @@ -1,16 +1,13 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.commons.MathTools; import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFrameVector3D; public class AccelerationLimitedYoFrameVector3D extends YoFrameVector3D { @@ -30,46 +27,49 @@ public class AccelerationLimitedYoFrameVector3D extends YoFrameVector3D private final double dt; - private final FrameVector3D positionError = new FrameVector3D(); - private final FrameVector3D acceleration = new FrameVector3D(); + private final FrameVector3D positionError; + private final FrameVector3D acceleration; - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); maxRate.set(initialValue); return maxRate; } - private static DoubleProvider createMaxAccelerationYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + private static DoubleProvider createMaxAccelerationYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "MaxAcceleration" + nameSuffix, registry); maxRate.set(initialValue); return maxRate; } - public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, + public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt, FrameTuple3DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, maxRate, maxAcceleration, dt, rawPosition, rawPosition.getReferenceFrame()); } - public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, + public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, maxRate, maxAcceleration, dt, null, referenceFrame); } - public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double maxAcceleration, + public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double maxAcceleration, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), createMaxAccelerationYoDouble(namePrefix, nameSuffix, maxAcceleration, registry), dt, null, referenceFrame); } - private AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, + private AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt, FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); + positionError = new FrameVector3D(referenceFrame); + acceleration = new FrameVector3D(referenceFrame); + if (maxRate == null) maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); if (maxAcceleration == null) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java index 611acd53..2d3adb8c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java @@ -2,7 +2,7 @@ import us.ihmc.commons.MathTools; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -23,12 +23,12 @@ public class AccelerationLimitedYoVariable extends YoDouble private final DoubleProvider inputVariable; - public AccelerationLimitedYoVariable(String name, YoVariableRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt) + public AccelerationLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt) { this(name, registry, maxRate, maxAcceleration, null, dt); } - public AccelerationLimitedYoVariable(String name, YoVariableRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, + public AccelerationLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, DoubleProvider inputVariable, double dt) { super(name, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java index 1e0b1885..d9235435 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java @@ -2,7 +2,7 @@ import us.ihmc.yoVariables.parameters.DoubleParameter; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; /** * This class calculates the alpha value given a break frequency provider @@ -42,7 +42,7 @@ public AlphaBasedOnBreakFrequencyProvider(DoubleProvider breakFrequencyProvider, * @param registry Parent registry for the break frequency parameter * @param defaultBreakFrequency Default value for the break frequency */ - public AlphaBasedOnBreakFrequencyProvider(String name, double dt, YoVariableRegistry registry, double defaultBreakFrequency) + public AlphaBasedOnBreakFrequencyProvider(String name, double dt, YoRegistry registry, double defaultBreakFrequency) { this(new DoubleParameter(name, registry, defaultBreakFrequency), dt); } @@ -54,7 +54,7 @@ public AlphaBasedOnBreakFrequencyProvider(String name, double dt, YoVariableRegi * @param dt Time step * @param registry Parent registry for the break frequency parameter */ - public AlphaBasedOnBreakFrequencyProvider(String name, double dt, YoVariableRegistry registry) + public AlphaBasedOnBreakFrequencyProvider(String name, double dt, YoRegistry registry) { this.breakFrequencyProvider = new DoubleParameter(name, registry, Double.NaN); this.dt = dt; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBetaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBetaFilteredYoVariable.java index fcec3b6a..58a221b7 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBetaFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBetaFilteredYoVariable.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; /** @@ -51,7 +51,7 @@ public class AlphaBetaFilteredYoVariable extends YoDouble private final YoDouble positionState; private final YoDouble xMeasuredVariable; - public AlphaBetaFilteredYoVariable(String name, YoVariableRegistry registry, double alpha, double beta, YoDouble positionVariable, + public AlphaBetaFilteredYoVariable(String name, YoRegistry registry, double alpha, double beta, YoDouble positionVariable, YoDouble xMeasuredVariable, double DT) { super(name, registry); @@ -64,7 +64,7 @@ public AlphaBetaFilteredYoVariable(String name, YoVariableRegistry registry, dou reset(); } - public AlphaBetaFilteredYoVariable(String name, YoVariableRegistry registry, YoDouble alphaVariable, YoDouble betaVariable, + public AlphaBetaFilteredYoVariable(String name, YoRegistry registry, YoDouble alphaVariable, YoDouble betaVariable, YoDouble positionVariable, YoDouble xMeasuredVariable, double DT) { super(name, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java index 4617a1d3..b204d605 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java @@ -2,7 +2,7 @@ import us.ihmc.commons.MathTools; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; public class AlphaFilteredWrappingYoVariable extends AlphaFilteredYoVariable @@ -22,7 +22,7 @@ public class AlphaFilteredWrappingYoVariable extends AlphaFilteredYoVariable //wrap the values in [lowerLimit ; upperLimit[ - public AlphaFilteredWrappingYoVariable(String name, String description, YoVariableRegistry registry, final YoDouble unfilteredVariable, DoubleProvider alphaVariable, double lowerLimit, double upperLimit) + public AlphaFilteredWrappingYoVariable(String name, String description, YoRegistry registry, final YoDouble unfilteredVariable, DoubleProvider alphaVariable, double lowerLimit, double upperLimit) { super(name, description, registry, alphaVariable); this.alphaVariable = alphaVariable; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java index df18443a..b4aaf0c3 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java @@ -4,10 +4,10 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoFramePoint3D; public class AlphaFilteredYoFramePoint extends YoFramePoint3D { @@ -18,11 +18,11 @@ public class AlphaFilteredYoFramePoint extends YoFramePoint3D /** * @deprecated Use - * {@link #AlphaFilteredYoFramePoint(String, String, YoVariableRegistry, double, ReferenceFrame)} + * {@link #AlphaFilteredYoFramePoint(String, String, YoRegistry, double, ReferenceFrame)} * instead. */ @Deprecated - public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) { return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, referenceFrame); @@ -30,11 +30,11 @@ public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String n /** * @deprecated Use - * {@link #AlphaFilteredYoFramePoint(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * {@link #AlphaFilteredYoFramePoint(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} * instead. */ @Deprecated - public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, referenceFrame); @@ -42,11 +42,11 @@ public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String n /** * @deprecated Use - * {@link #AlphaFilteredYoFramePoint(String, String, YoVariableRegistry, double, FrameTuple3DReadOnly)} + * {@link #AlphaFilteredYoFramePoint(String, String, YoRegistry, double, FrameTuple3DReadOnly)} * instead. */ @Deprecated - public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) { return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); @@ -54,40 +54,40 @@ public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String n /** * @deprecated Use - * {@link #AlphaFilteredYoFramePoint(String, String, YoVariableRegistry, DoubleProvider, FrameTuple3DReadOnly)} + * {@link #AlphaFilteredYoFramePoint(String, String, YoRegistry, DoubleProvider, FrameTuple3DReadOnly)} * instead. */ @Deprecated - public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) { return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); } - public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); } - public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); } - public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) { this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); } - public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, + public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) { this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); } - private AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + private AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, FrameTuple3DReadOnly unfilteredFrameTuple3D) { super(namePrefix, nameSuffix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java index 0e2fe1b5..3cf1ff84 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java @@ -4,10 +4,10 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; import us.ihmc.euclid.tuple2D.Point2D; import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoFramePoint2D; public class AlphaFilteredYoFramePoint2d extends YoFramePoint2D { @@ -18,11 +18,11 @@ public class AlphaFilteredYoFramePoint2d extends YoFramePoint2D /** * @deprecated Use - * {@link #AlphaFilteredYoFramePoint2d(String, String, YoVariableRegistry, double, ReferenceFrame)} + * {@link #AlphaFilteredYoFramePoint2d(String, String, YoRegistry, double, ReferenceFrame)} * instead. */ @Deprecated - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) { return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); @@ -30,11 +30,11 @@ public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(Stri /** * @deprecated Use - * {@link #AlphaFilteredYoFramePoint2d(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * {@link #AlphaFilteredYoFramePoint2d(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} * instead. */ @Deprecated - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { return createAlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); @@ -42,23 +42,23 @@ public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(Stri /** * @deprecated Use - * {@link #AlphaFilteredYoFramePoint2d(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * {@link #AlphaFilteredYoFramePoint2d(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} * instead. */ @Deprecated public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, String description, - YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); } /** * @deprecated Use - * {@link #AlphaFilteredYoFramePoint2d(String, String, YoVariableRegistry, double, FrameTuple2DReadOnly)} + * {@link #AlphaFilteredYoFramePoint2d(String, String, YoRegistry, double, FrameTuple2DReadOnly)} * instead. */ @Deprecated - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) { return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); @@ -66,40 +66,40 @@ public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(Stri /** * @deprecated Use - * {@link #AlphaFilteredYoFramePoint2d(String, String, YoVariableRegistry, DoubleProvider, FrameTuple2DReadOnly)} + * {@link #AlphaFilteredYoFramePoint2d(String, String, YoRegistry, DoubleProvider, FrameTuple2DReadOnly)} * instead. */ @Deprecated - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) { return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); } - public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); } - public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); } - public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) { this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); } - public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, + public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) { this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); } - private AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + private AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, FrameTuple2DReadOnly unfilteredFrameTuple2D) { super(namePrefix, nameSuffix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java index b4d20031..d19a7b04 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java @@ -5,11 +5,11 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFrameQuaternion; public class AlphaFilteredYoFrameQuaternion extends YoFrameQuaternion implements ProcessingYoVariable { @@ -20,7 +20,7 @@ public class AlphaFilteredYoFrameQuaternion extends YoFrameQuaternion implements private final Quaternion qPreviousFiltered = new Quaternion(); private final Quaternion qNewFiltered = new Quaternion(); - private static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoVariableRegistry registry) + private static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "AlphaVariable", registry); maxRate.set(initialValue); @@ -28,25 +28,25 @@ private static DoubleProvider createAlphaYoDouble(String namePrefix, double init } public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, double alpha, - YoVariableRegistry registry) + YoRegistry registry) { this(namePrefix, nameSuffix, unfilteredQuaternion, createAlphaYoDouble(namePrefix, alpha, registry), registry); } public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, DoubleProvider alpha, ReferenceFrame referenceFrame, - YoVariableRegistry registry) + YoRegistry registry) { this(namePrefix, nameSuffix, null, alpha, referenceFrame, registry); } public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, DoubleProvider alpha, - YoVariableRegistry registry) + YoRegistry registry) { this(namePrefix, nameSuffix, unfilteredQuaternion, alpha, unfilteredQuaternion.getReferenceFrame(), registry); } private AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, DoubleProvider alpha, - ReferenceFrame referenceFrame, YoVariableRegistry registry) + ReferenceFrame referenceFrame, YoRegistry registry) { super(namePrefix, nameSuffix, referenceFrame, registry); this.unfilteredQuaternion = unfilteredQuaternion; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java index 1ef2baf5..dcb4bba8 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java @@ -4,10 +4,10 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoFrameVector3D; public class AlphaFilteredYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable { @@ -18,11 +18,11 @@ public class AlphaFilteredYoFrameVector extends YoFrameVector3D implements Proce /** * @deprecated Use - * {@link #AlphaFilteredYoFrameVector(String, String, YoVariableRegistry, double, ReferenceFrame)} + * {@link #AlphaFilteredYoFrameVector(String, String, YoRegistry, double, ReferenceFrame)} * instead. */ @Deprecated - public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) { return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, referenceFrame); @@ -30,11 +30,11 @@ public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String /** * @deprecated Use - * {@link #AlphaFilteredYoFrameVector(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * {@link #AlphaFilteredYoFrameVector(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} * instead. */ @Deprecated - public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, referenceFrame); @@ -42,11 +42,11 @@ public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String /** * @deprecated Use - * {@link #AlphaFilteredYoFrameVector(String, String, YoVariableRegistry, double, FrameTuple3DReadOnly)} + * {@link #AlphaFilteredYoFrameVector(String, String, YoRegistry, double, FrameTuple3DReadOnly)} * instead. */ @Deprecated - public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) { return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); @@ -54,40 +54,40 @@ public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String /** * @deprecated Use - * {@link #AlphaFilteredYoFrameVector(String, String, YoVariableRegistry, DoubleProvider, FrameTuple3DReadOnly)} + * {@link #AlphaFilteredYoFrameVector(String, String, YoRegistry, DoubleProvider, FrameTuple3DReadOnly)} * instead. */ @Deprecated - public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) { return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); } - public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); } - public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); } - public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) { this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); } - public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, + public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) { this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); } - private AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + private AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, FrameTuple3DReadOnly unfilteredFrameTuple3D) { super(namePrefix, nameSuffix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java index 6bba4040..179797e8 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java @@ -4,10 +4,10 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; import us.ihmc.euclid.tuple2D.Vector2D; import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoFrameVector2D; public class AlphaFilteredYoFrameVector2d extends YoFrameVector2D { @@ -18,11 +18,11 @@ public class AlphaFilteredYoFrameVector2d extends YoFrameVector2D /** * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, double, ReferenceFrame)} + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, double, ReferenceFrame)} * instead. */ @Deprecated - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) { return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); @@ -30,11 +30,11 @@ public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(St /** * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} * instead. */ @Deprecated - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); @@ -42,12 +42,12 @@ public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(St /** * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, DoubleProvider, ReferenceFrame)} + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} * instead. */ @Deprecated public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, - YoVariableRegistry registry, DoubleProvider alpha, + YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); @@ -55,11 +55,11 @@ public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(St /** * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, double, FrameTuple2DReadOnly)} + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, double, FrameTuple2DReadOnly)} * instead. */ @Deprecated - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) { return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); @@ -67,11 +67,11 @@ public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(St /** * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, DoubleProvider, FrameTuple2DReadOnly)} + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, DoubleProvider, FrameTuple2DReadOnly)} * instead. */ @Deprecated - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) { return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, unfilteredFrameTuple2D); @@ -79,41 +79,41 @@ public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(St /** * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoVariableRegistry, DoubleProvider, FrameTuple2DReadOnly)} + * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, DoubleProvider, FrameTuple2DReadOnly)} * instead. */ @Deprecated public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, - YoVariableRegistry registry, DoubleProvider alpha, + YoRegistry registry, DoubleProvider alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) { return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); } - public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, ReferenceFrame referenceFrame) + public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); } - public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); } - public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, + public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) { this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); } - public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, + public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) { this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); } - private AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + private AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, FrameTuple2DReadOnly unfilteredFrameTuple2D) { super(namePrefix, nameSuffix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java index 501b0e38..9477e7ed 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java @@ -2,7 +2,7 @@ import us.ihmc.commons.MathTools; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -35,39 +35,39 @@ public class AlphaFilteredYoVariable extends YoDouble implements ProcessingYoVar private final YoDouble position; protected final YoBoolean hasBeenCalled; - public static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoVariableRegistry registry) + public static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "AlphaVariable", registry); maxRate.set(initialValue); return maxRate; } - public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, double alpha) + public AlphaFilteredYoVariable(String name, YoRegistry registry, double alpha) { this(name, registry, alpha, null); } - public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, double alpha, YoDouble positionVariable) + public AlphaFilteredYoVariable(String name, YoRegistry registry, double alpha, YoDouble positionVariable) { this(name, "", registry, createAlphaYoDouble(name, alpha, registry), positionVariable); } - public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, DoubleProvider alphaVariable) + public AlphaFilteredYoVariable(String name, YoRegistry registry, DoubleProvider alphaVariable) { this(name, "", registry, alphaVariable, null); } - public AlphaFilteredYoVariable(String name, String description, YoVariableRegistry registry, DoubleProvider alphaVariable) + public AlphaFilteredYoVariable(String name, String description, YoRegistry registry, DoubleProvider alphaVariable) { this(name, description, registry, alphaVariable, null); } - public AlphaFilteredYoVariable(String name, YoVariableRegistry registry, DoubleProvider alphaVariable, YoDouble positionVariable) + public AlphaFilteredYoVariable(String name, YoRegistry registry, DoubleProvider alphaVariable, YoDouble positionVariable) { this(name, "", registry, alphaVariable, positionVariable); } - public AlphaFilteredYoVariable(String name, String description, YoVariableRegistry registry, DoubleProvider alphaVariable, YoDouble positionVariable) + public AlphaFilteredYoVariable(String name, String description, YoRegistry registry, DoubleProvider alphaVariable, YoDouble positionVariable) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", description, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java index 608da21b..ce05ef8b 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java @@ -1,23 +1,23 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFramePoint3D; -import us.ihmc.yoVariables.variable.YoFrameVector3D; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; public class BacklashCompensatingVelocityYoFrameVector extends YoFrameVector3D { private final BacklashCompensatingVelocityYoVariable xDot, yDot, zDot; public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensatingVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, YoDouble slopTime, - YoVariableRegistry registry, YoFrameVector3D yoFrameVectorToDifferentiate) + YoRegistry registry, YoFrameVector3D yoFrameVectorToDifferentiate) { - BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoX(), dt, slopTime, + BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoX(), dt, slopTime, registry); - BacklashCompensatingVelocityYoVariable yDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoY(), dt, slopTime, + BacklashCompensatingVelocityYoVariable yDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoY(), dt, slopTime, registry); - BacklashCompensatingVelocityYoVariable zDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoZ(), dt, slopTime, + BacklashCompensatingVelocityYoVariable zDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoZ(), dt, slopTime, registry); BacklashCompensatingVelocityYoFrameVector ret = new BacklashCompensatingVelocityYoFrameVector(xDot, yDot, zDot, registry, yoFrameVectorToDifferentiate); @@ -26,13 +26,13 @@ public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensati } public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensatingVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, YoDouble slopTime, - YoVariableRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) + YoRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) { - BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoX(), dt, slopTime, + BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoX(), dt, slopTime, registry); - BacklashCompensatingVelocityYoVariable yDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoY(), dt, slopTime, + BacklashCompensatingVelocityYoVariable yDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoY(), dt, slopTime, registry); - BacklashCompensatingVelocityYoVariable zDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoZ(), dt, slopTime, + BacklashCompensatingVelocityYoVariable zDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoZ(), dt, slopTime, registry); BacklashCompensatingVelocityYoFrameVector ret = new BacklashCompensatingVelocityYoFrameVector(xDot, yDot, zDot, registry, yoFramePointToDifferentiate); @@ -41,7 +41,7 @@ public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensati } private BacklashCompensatingVelocityYoFrameVector(BacklashCompensatingVelocityYoVariable xDot, BacklashCompensatingVelocityYoVariable yDot, BacklashCompensatingVelocityYoVariable zDot, - YoVariableRegistry registry, YoFrameVector3D yoFrameVectorToDifferentiate) + YoRegistry registry, YoFrameVector3D yoFrameVectorToDifferentiate) { super(xDot, yDot, zDot, yoFrameVectorToDifferentiate.getReferenceFrame()); @@ -51,7 +51,7 @@ private BacklashCompensatingVelocityYoFrameVector(BacklashCompensatingVelocityYo } private BacklashCompensatingVelocityYoFrameVector(BacklashCompensatingVelocityYoVariable xDot, BacklashCompensatingVelocityYoVariable yDot, BacklashCompensatingVelocityYoVariable zDot, - YoVariableRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) + YoRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) { super(xDot, yDot, zDot, yoFramePointToDifferentiate.getReferenceFrame()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java index 356c93fa..81354164 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java @@ -1,7 +1,7 @@ package us.ihmc.robotics.math.filters; import us.ihmc.commons.MathTools; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoEnum; @@ -22,7 +22,7 @@ public class BacklashCompensatingVelocityYoVariable extends YoDouble implements private final YoDouble timeInState; public BacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, YoDouble positionVariable, double dt, - YoDouble slopTime, YoVariableRegistry registry) + YoDouble slopTime, YoRegistry registry) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); @@ -44,7 +44,7 @@ public BacklashCompensatingVelocityYoVariable(String name, String description, Y } public BacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, double dt, YoDouble slopTime, - YoVariableRegistry registry) + YoRegistry registry) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java index 63165b6c..85488225 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java @@ -2,7 +2,7 @@ import us.ihmc.commons.MathTools; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoEnum; @@ -24,13 +24,13 @@ public class BacklashProcessingYoVariable extends YoDouble implements Processing private final double dt; - public BacklashProcessingYoVariable(String name, String description, double dt, DoubleProvider slopTime, YoVariableRegistry registry) + public BacklashProcessingYoVariable(String name, String description, double dt, DoubleProvider slopTime, YoRegistry registry) { this(name, description, null, dt, slopTime, registry); } public BacklashProcessingYoVariable(String name, String description, YoDouble velocityVariable, double dt, DoubleProvider slopTime, - YoVariableRegistry registry) + YoRegistry registry) { super(name, description, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java index bede1b2c..80bc29b1 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java @@ -3,9 +3,9 @@ import us.ihmc.euclid.referenceFrame.FramePoint2D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple2D.Point2D; -import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoFramePoint2D; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; public class BetaFilteredYoFramePoint2d extends YoFramePoint2D { @@ -19,33 +19,33 @@ private BetaFilteredYoFramePoint2d(BetaFilteredYoVariable x, BetaFilteredYoVaria this.y = y; } - public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, ReferenceFrame referenceFrame) + public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, int beta, ReferenceFrame referenceFrame) { // beta is a int - BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta); - BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta); + BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta); + BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta); BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, referenceFrame); return ret; } - public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, String description, YoVariableRegistry registry, int beta, ReferenceFrame referenceFrame) + public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, String description, YoRegistry registry, int beta, ReferenceFrame referenceFrame) { // beta is a int - BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), description, registry, beta); - BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), description, registry, beta); + BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), description, registry, beta); + BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), description, registry, beta); BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, referenceFrame); return ret; } - public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFramePoint2D unfilteredPoint) + public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, int beta, YoFramePoint2D unfilteredPoint) { // beta is a int - BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoX()); - BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoY()); + BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoX()); + BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoY()); BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, unfilteredPoint.getReferenceFrame()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java index 681a9a5e..f65b2beb 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java @@ -3,9 +3,9 @@ import us.ihmc.euclid.referenceFrame.FrameVector2D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple2D.Vector2D; -import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoFrameVector2D; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; public class BetaFilteredYoFrameVector2d extends YoFrameVector2D { @@ -19,22 +19,22 @@ private BetaFilteredYoFrameVector2d(BetaFilteredYoVariable x, BetaFilteredYoVari this.y = y; } - public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, ReferenceFrame referenceFrame) + public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, int beta, ReferenceFrame referenceFrame) { // beta is a int - BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta); - BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta); + BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta); + BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta); BetaFilteredYoFrameVector2d ret = new BetaFilteredYoFrameVector2d(x, y, referenceFrame); return ret; } - public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFrameVector2D unfilteredVector) + public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, int beta, YoFrameVector2D unfilteredVector) { // beta is a int - BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoX()); - BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoY()); + BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoX()); + BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoY()); BetaFilteredYoFrameVector2d ret = new BetaFilteredYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java index 464695c6..4fc3fcc7 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -33,22 +33,22 @@ public class BetaFilteredYoVariable extends YoDouble private final YoBoolean hasBeenCalled; - public BetaFilteredYoVariable(String name, YoVariableRegistry registry, int beta) + public BetaFilteredYoVariable(String name, YoRegistry registry, int beta) { this(name, "", registry, beta, null); } - public BetaFilteredYoVariable(String name, String description, YoVariableRegistry registry, int beta) + public BetaFilteredYoVariable(String name, String description, YoRegistry registry, int beta) { this(name, description, registry, beta, null); } - public BetaFilteredYoVariable(String name, YoVariableRegistry registry, int beta, YoDouble positionVariable) + public BetaFilteredYoVariable(String name, YoRegistry registry, int beta, YoDouble positionVariable) { this(name, "", registry, beta, positionVariable); } - public BetaFilteredYoVariable(String name, String description, YoVariableRegistry registry, int beta, YoDouble positionVariable) + public BetaFilteredYoVariable(String name, String description, YoRegistry registry, int beta, YoDouble positionVariable) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java index e33a759a..419f806b 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java @@ -2,11 +2,11 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameTuple3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFrameTuple3D; -import us.ihmc.yoVariables.variable.YoFrameVector3D; public class DeadzoneYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable { @@ -21,32 +21,32 @@ private DeadzoneYoFrameVector(DeadzoneYoVariable x, DeadzoneYoVariable y, Deadzo this.z = z; } - public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, YoVariableRegistry registry, YoDouble deadzoneSize, ReferenceFrame referenceFrame) + public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, YoRegistry registry, YoDouble deadzoneSize, ReferenceFrame referenceFrame) { return createDeadzoneYoFrameVector(namePrefix, "", registry, deadzoneSize, referenceFrame); } - public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble deadzoneSize, ReferenceFrame referenceFrame) + public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, YoDouble deadzoneSize, ReferenceFrame referenceFrame) { - DeadzoneYoVariable x = new DeadzoneYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), deadzoneSize, registry); - DeadzoneYoVariable y = new DeadzoneYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), deadzoneSize, registry); - DeadzoneYoVariable z = new DeadzoneYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), deadzoneSize, registry); + DeadzoneYoVariable x = new DeadzoneYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), deadzoneSize, registry); + DeadzoneYoVariable y = new DeadzoneYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), deadzoneSize, registry); + DeadzoneYoVariable z = new DeadzoneYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), deadzoneSize, registry); DeadzoneYoFrameVector ret = new DeadzoneYoFrameVector(x, y, z, referenceFrame); return ret; } - public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, YoVariableRegistry registry, YoDouble deadzoneSize, YoFrameTuple3D rawTuple) + public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, YoRegistry registry, YoDouble deadzoneSize, YoFrameTuple3D rawTuple) { return createDeadzoneYoFrameVector(namePrefix, "", registry, deadzoneSize, rawTuple); } - public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble deadzoneSize, YoFrameTuple3D rawTuple) + public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, YoDouble deadzoneSize, YoFrameTuple3D rawTuple) { - DeadzoneYoVariable x = new DeadzoneYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), rawTuple.getYoX(), deadzoneSize, registry); - DeadzoneYoVariable y = new DeadzoneYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), rawTuple.getYoY(), deadzoneSize, registry); - DeadzoneYoVariable z = new DeadzoneYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), rawTuple.getYoZ(), deadzoneSize, registry); + DeadzoneYoVariable x = new DeadzoneYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), rawTuple.getYoX(), deadzoneSize, registry); + DeadzoneYoVariable y = new DeadzoneYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), rawTuple.getYoY(), deadzoneSize, registry); + DeadzoneYoVariable z = new DeadzoneYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), rawTuple.getYoZ(), deadzoneSize, registry); DeadzoneYoFrameVector ret = new DeadzoneYoFrameVector(x, y, z, rawTuple.getReferenceFrame()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java index f49b922f..c7e93205 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; public class DeadzoneYoVariable extends YoDouble implements ProcessingYoVariable @@ -8,14 +8,14 @@ public class DeadzoneYoVariable extends YoDouble implements ProcessingYoVariable private final YoDouble deadzoneSize; private final YoDouble inputVariable; - public DeadzoneYoVariable(String name, YoDouble deadzoneSize, YoVariableRegistry registry) + public DeadzoneYoVariable(String name, YoDouble deadzoneSize, YoRegistry registry) { super(name, registry); this.inputVariable = null; this.deadzoneSize = deadzoneSize; } - public DeadzoneYoVariable(String name, YoDouble inputVariable, YoDouble deadzoneSize, YoVariableRegistry registry) + public DeadzoneYoVariable(String name, YoDouble inputVariable, YoDouble deadzoneSize, YoRegistry registry) { super(name, registry); this.inputVariable = inputVariable; @@ -48,7 +48,7 @@ else if (valueToBeCorrected <= -deadzoneSize.getDoubleValue()) public static void main(String[] args) { - YoVariableRegistry registry = new YoVariableRegistry("test"); + YoRegistry registry = new YoRegistry("test"); YoDouble deadzoneSize = new YoDouble("deadzoneSize", registry); YoDouble input = new YoDouble("input", registry); deadzoneSize.set(2.0); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java index e010bacf..0d7c554f 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; public class DelayedYoBoolean extends YoBoolean @@ -9,7 +9,7 @@ public class DelayedYoBoolean extends YoBoolean private final YoBoolean[] previousYoVariables; - public DelayedYoBoolean(String name, String description, YoBoolean variableToDelay, int ticksToDelay, YoVariableRegistry registry) + public DelayedYoBoolean(String name, String description, YoBoolean variableToDelay, int ticksToDelay, YoRegistry registry) { super(name, description, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java index b87555d6..391f2239 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; public class DelayedYoDouble extends YoDouble @@ -9,7 +9,7 @@ public class DelayedYoDouble extends YoDouble private final YoDouble[] previousYoVariables; - public DelayedYoDouble(String name, String description, YoDouble variableToDelay, int ticksToDelay, YoVariableRegistry registry) + public DelayedYoDouble(String name, String description, YoDouble variableToDelay, int ticksToDelay, YoRegistry registry) { super(name, description, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java index 5ab166ef..5bd56a4c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -15,7 +15,7 @@ public class DeltaLimitedYoVariable extends YoDouble private final YoDouble desired; private final YoBoolean isLimitingActive; - public DeltaLimitedYoVariable(String name, YoVariableRegistry registry, double maxDelta) + public DeltaLimitedYoVariable(String name, YoRegistry registry, double maxDelta) { super(name, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java index f93d052b..6a0ba403 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoEnum; @@ -35,7 +35,7 @@ public class FilteredDiscreteVelocityYoVariable extends YoDouble private final YoDouble lastPosition; private boolean hasBeenCalled; - public FilteredDiscreteVelocityYoVariable(String name, String description, double alpha, YoDouble time, YoVariableRegistry registry) + public FilteredDiscreteVelocityYoVariable(String name, String description, double alpha, YoDouble time, YoRegistry registry) { super(name, description, registry); @@ -47,14 +47,14 @@ public FilteredDiscreteVelocityYoVariable(String name, String description, doubl lastPosition = new YoDouble(name + "_lastPosition", registry); lastUpdateTime = new YoDouble(name + "_lastUpdateTime", registry); - lastUpdateDirection = YoEnum.create(name + "_lastUpdateDirection", Direction.class, registry); + lastUpdateDirection = new YoEnum<>(name + "_lastUpdateDirection", registry, Direction.class); unfilteredVelocity = new YoDouble(name + "_unfilteredVelocity", registry); reset(); } public FilteredDiscreteVelocityYoVariable(String name, String description, double alpha, YoDouble positionVariable, YoDouble time, - YoVariableRegistry registry) + YoRegistry registry) { super(name, description, registry); @@ -67,14 +67,14 @@ public FilteredDiscreteVelocityYoVariable(String name, String description, doubl lastPosition = new YoDouble(name + "_lastPosition", registry); lastUpdateTime = new YoDouble(name + "_lastUpdateTime", registry); - lastUpdateDirection = YoEnum.create(name + "_lastUpdateDirection", Direction.class, registry); + lastUpdateDirection = new YoEnum<>(name + "_lastUpdateDirection", registry, Direction.class); unfilteredVelocity = new YoDouble(name + "_unfilteredVelocity", registry); reset(); } public FilteredDiscreteVelocityYoVariable(String name, String description, YoDouble alphaVariable, YoDouble positionVariable, - YoDouble time, YoVariableRegistry registry) + YoDouble time, YoRegistry registry) { super(name, description, registry); position = positionVariable; @@ -85,7 +85,7 @@ public FilteredDiscreteVelocityYoVariable(String name, String description, YoDou lastPosition = new YoDouble(name + "_lastPosition", registry); lastUpdateTime = new YoDouble(name + "_lastUpdateTime", registry); - lastUpdateDirection = YoEnum.create(name + "_lastUpdateDirection", Direction.class, registry); + lastUpdateDirection = new YoEnum<>(name + "_lastUpdateDirection", registry, Direction.class); unfilteredVelocity = new YoDouble(name + "_unfilteredVelocity", registry); reset(); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java index cf4c108d..369c49bc 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java @@ -4,10 +4,10 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoFrameVector3D; /** *

@@ -36,12 +36,12 @@ public class FilteredVelocityYoFrameVector extends YoFrameVector3D /** * @deprecated Use - * {@link #FilteredVelocityYoFrameVector(String, String, DoubleProvider, double, YoVariableRegistry, FrameTuple3DReadOnly)} + * {@link #FilteredVelocityYoFrameVector(String, String, DoubleProvider, double, YoRegistry, FrameTuple3DReadOnly)} * instead. */ @Deprecated public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, - YoVariableRegistry registry, + YoRegistry registry, FrameTuple3DReadOnly frameTuple3DToDifferentiate) { return new FilteredVelocityYoFrameVector(namePrefix, nameSuffix, alpha, dt, registry, frameTuple3DToDifferentiate); @@ -49,17 +49,17 @@ public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector( /** * @deprecated Use - * {@link #FilteredVelocityYoFrameVector(String, String, DoubleProvider, double, YoVariableRegistry, ReferenceFrame)} + * {@link #FilteredVelocityYoFrameVector(String, String, DoubleProvider, double, YoRegistry, ReferenceFrame)} * instead. */ @Deprecated public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, - YoVariableRegistry registry, ReferenceFrame referenceFrame) + YoRegistry registry, ReferenceFrame referenceFrame) { return new FilteredVelocityYoFrameVector(namePrefix, nameSuffix, alpha, dt, registry, referenceFrame); } - public FilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, + public FilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoRegistry registry, FrameTuple3DReadOnly frameTuple3DToDifferentiate) { super(namePrefix, nameSuffix, frameTuple3DToDifferentiate.getReferenceFrame(), registry); @@ -72,7 +72,7 @@ public FilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, Doubl reset(); } - public FilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, + public FilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoRegistry registry, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java index 250f1faa..ab519961 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java @@ -4,10 +4,10 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; import us.ihmc.euclid.tuple2D.Vector2D; import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoFrameVector2D; public class FilteredVelocityYoFrameVector2d extends YoFrameVector2D { @@ -20,12 +20,12 @@ public class FilteredVelocityYoFrameVector2d extends YoFrameVector2D /** * @deprecated Use - * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoVariableRegistry, FrameTuple2DReadOnly)} + * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoRegistry, FrameTuple2DReadOnly)} * instead. */ @Deprecated public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, - YoVariableRegistry registry, + YoRegistry registry, FrameTuple2DReadOnly frameTuple2DToDifferentiate) { return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, frameTuple2DToDifferentiate); @@ -33,12 +33,12 @@ public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVecto /** * @deprecated Use - * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoVariableRegistry, FrameTuple2DReadOnly)} + * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoRegistry, FrameTuple2DReadOnly)} * instead. */ @Deprecated public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, String description, - DoubleProvider alpha, double dt, YoVariableRegistry registry, + DoubleProvider alpha, double dt, YoRegistry registry, FrameTuple2DReadOnly frameTuple2DToDifferentiate) { return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, frameTuple2DToDifferentiate); @@ -46,17 +46,17 @@ public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVecto /** * @deprecated Use - * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoVariableRegistry, ReferenceFrame)} + * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoRegistry, ReferenceFrame)} * instead. */ @Deprecated public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, - YoVariableRegistry registry, ReferenceFrame referenceFrame) + YoRegistry registry, ReferenceFrame referenceFrame) { return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, referenceFrame); } - public FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, + public FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoRegistry registry, FrameTuple2DReadOnly frameTuple2DToDifferentiate) { super(namePrefix, nameSuffix, frameTuple2DToDifferentiate.getReferenceFrame(), registry); @@ -69,7 +69,7 @@ public FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, Dou reset(); } - public FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoVariableRegistry registry, + public FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoRegistry registry, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java index 444c7710..f8bd502c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java @@ -2,7 +2,7 @@ import us.ihmc.robotics.geometry.AngleTools; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -33,7 +33,7 @@ public class FilteredVelocityYoVariable extends YoDouble implements ProcessingYo private final YoDouble lastPosition; private final YoBoolean hasBeenCalled; - public FilteredVelocityYoVariable(String name, String description, double alpha, double dt, YoVariableRegistry registry) + public FilteredVelocityYoVariable(String name, String description, double alpha, double dt, YoRegistry registry) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); @@ -49,7 +49,7 @@ public FilteredVelocityYoVariable(String name, String description, double alpha, reset(); } - public FilteredVelocityYoVariable(String name, String description, double alpha, YoDouble positionVariable, double dt, YoVariableRegistry registry) + public FilteredVelocityYoVariable(String name, String description, double alpha, YoDouble positionVariable, double dt, YoRegistry registry) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); @@ -66,7 +66,7 @@ public FilteredVelocityYoVariable(String name, String description, double alpha, reset(); } - public FilteredVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, YoDouble positionVariable, double dt, YoVariableRegistry registry) + public FilteredVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, YoDouble positionVariable, double dt, YoRegistry registry) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); @@ -82,7 +82,7 @@ public FilteredVelocityYoVariable(String name, String description, DoubleProvide reset(); } - public FilteredVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, double dt, YoVariableRegistry registry) + public FilteredVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, double dt, YoRegistry registry) { super(name, description, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java index 18067e7f..0f97d89a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java @@ -6,10 +6,10 @@ import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoFrameQuaternion; -import us.ihmc.yoVariables.variable.YoFrameVector3D; public class FiniteDifferenceAngularVelocityYoFrameVector extends YoFrameVector3D { @@ -25,17 +25,17 @@ public class FiniteDifferenceAngularVelocityYoFrameVector extends YoFrameVector3 private final double dt; - public FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, ReferenceFrame referenceFrame, double dt, YoVariableRegistry registry) + public FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, ReferenceFrame referenceFrame, double dt, YoRegistry registry) { this(namePrefix, null, referenceFrame, dt, registry); } - public FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, YoFrameQuaternion orientationToDifferentiate, double dt, YoVariableRegistry registry) + public FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, YoFrameQuaternion orientationToDifferentiate, double dt, YoRegistry registry) { this(namePrefix, orientationToDifferentiate, orientationToDifferentiate.getReferenceFrame(), dt, registry); } - private FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, YoFrameQuaternion orientationToDifferentiate, ReferenceFrame referenceFrame, double dt, YoVariableRegistry registry) + private FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, YoFrameQuaternion orientationToDifferentiate, ReferenceFrame referenceFrame, double dt, YoRegistry registry) { super(namePrefix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java index 636ba048..ee9be329 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; public class FirstOrderBandPassFilteredYoVariable extends FirstOrderFilteredYoVariable @@ -10,7 +10,7 @@ public class FirstOrderBandPassFilteredYoVariable extends FirstOrderFilteredYoVa private final FirstOrderFilteredYoVariable highPassFilteredInput; public FirstOrderBandPassFilteredYoVariable(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz, - YoDouble yoTime, YoVariableRegistry registry) + YoDouble yoTime, YoRegistry registry) { super(name, description, maxPassThroughFrequency_Hz, yoTime, FirstOrderFilterType.LOW_PASS, registry); @@ -21,7 +21,7 @@ public FirstOrderBandPassFilteredYoVariable(String name, String description, dou } public FirstOrderBandPassFilteredYoVariable(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz, - double DT, YoVariableRegistry registry) + double DT, YoRegistry registry) { super(name, description, maxPassThroughFrequency_Hz, DT, FirstOrderFilterType.LOW_PASS, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java index d1c7f545..ab2caa2f 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; public class FirstOrderFilteredYoVariable extends YoDouble @@ -23,7 +23,7 @@ public enum FirstOrderFilterType private FirstOrderFilterType highOrLowPass; - public FirstOrderFilteredYoVariable(String name, String description, double cutoffFrequency_Hz, YoDouble yoTime, FirstOrderFilterType highOrLowPass, YoVariableRegistry registry) + public FirstOrderFilteredYoVariable(String name, String description, double cutoffFrequency_Hz, YoDouble yoTime, FirstOrderFilterType highOrLowPass, YoRegistry registry) { super(name, description, registry); @@ -48,7 +48,7 @@ public FirstOrderFilteredYoVariable(String name, String description, double cuto this.highOrLowPass = highOrLowPass; } - public FirstOrderFilteredYoVariable(String name, String description, double cutoffFrequency_Hz, double DT, FirstOrderFilterType highOrLowPass, YoVariableRegistry registry) + public FirstOrderFilteredYoVariable(String name, String description, double cutoffFrequency_Hz, double DT, FirstOrderFilterType highOrLowPass, YoRegistry registry) { this(name, description, cutoffFrequency_Hz, null, highOrLowPass, registry); this.dt = DT; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java index 1cbfaaad..0f85d3cf 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoInteger; @@ -13,13 +13,13 @@ public class GlitchFilteredYoBoolean extends YoBoolean public GlitchFilteredYoBoolean(String name, int windowSize) { super(name, "GlitchFilteredYoBoolean", null); - counter = new YoInteger(name + "Count", getYoVariableRegistry()); - this.windowSize = new YoInteger(name + "WindowSize", getYoVariableRegistry()); + counter = new YoInteger(name + "Count", getRegistry()); + this.windowSize = new YoInteger(name + "WindowSize", getRegistry()); initialize(null, windowSize); } - public GlitchFilteredYoBoolean(String name, YoVariableRegistry registry, int windowSize) + public GlitchFilteredYoBoolean(String name, YoRegistry registry, int windowSize) { super(name, registry); counter = new YoInteger(name + "Count", registry); @@ -31,18 +31,18 @@ public GlitchFilteredYoBoolean(String name, YoVariableRegistry registry, int win public GlitchFilteredYoBoolean(String name, YoBoolean yoVariableToFilter, int windowSize) { super(name, "GlitchFilteredYoBoolean", null); - counter = new YoInteger(name + "Count", getYoVariableRegistry()); - this.windowSize = new YoInteger(name + "WindowSize", getYoVariableRegistry()); + counter = new YoInteger(name + "Count", getRegistry()); + this.windowSize = new YoInteger(name + "WindowSize", getRegistry()); initialize(yoVariableToFilter, windowSize); } - public GlitchFilteredYoBoolean(String name, YoVariableRegistry registry, YoBoolean yoVariableToFilter, int windowSize) + public GlitchFilteredYoBoolean(String name, YoRegistry registry, YoBoolean yoVariableToFilter, int windowSize) { this(name, "", registry, yoVariableToFilter, windowSize); } - public GlitchFilteredYoBoolean(String name, String description, YoVariableRegistry registry, YoBoolean yoVariableToFilter, int windowSize) + public GlitchFilteredYoBoolean(String name, String description, YoRegistry registry, YoBoolean yoVariableToFilter, int windowSize) { super(name, description, registry); counter = new YoInteger(name + "Count", description, registry); @@ -65,18 +65,18 @@ private void initialize(YoBoolean yoVariableToFilter, int windowSize) this.set(false); } - public void set(boolean value) + public boolean set(boolean value) { - super.set(value); if (counter != null) { counter.set(0); } + return super.set(value); } public void update(boolean value) { - + if (value != this.getBooleanValue()) { counter.set(counter.getIntegerValue() + 1); @@ -84,7 +84,7 @@ public void update(boolean value) else counter.set(0); - if (counter.getIntegerValue() >= (windowSize.getIntegerValue() )) + if (counter.getIntegerValue() >= (windowSize.getIntegerValue())) set(value); } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java index eed1c217..4ee14989 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java @@ -1,7 +1,7 @@ package us.ihmc.robotics.math.filters; import us.ihmc.yoVariables.providers.IntegerProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoInteger; public class GlitchFilteredYoInteger extends YoInteger @@ -11,12 +11,12 @@ public class GlitchFilteredYoInteger extends YoInteger private final IntegerProvider windowSize; private final YoInteger counter; - public GlitchFilteredYoInteger(String name, int windowSize, YoVariableRegistry registry) + public GlitchFilteredYoInteger(String name, int windowSize, YoRegistry registry) { this(name, windowSize, null, registry); } - public GlitchFilteredYoInteger(String name, int windowSize, YoInteger position, YoVariableRegistry registry) + public GlitchFilteredYoInteger(String name, int windowSize, YoInteger position, YoRegistry registry) { super(name, GlitchFilteredYoInteger.class.getSimpleName(), registry); @@ -29,12 +29,12 @@ public GlitchFilteredYoInteger(String name, int windowSize, YoInteger position, this.windowSize = yoWindowSize; } - public GlitchFilteredYoInteger(String name, IntegerProvider windowSize, YoVariableRegistry registry) + public GlitchFilteredYoInteger(String name, IntegerProvider windowSize, YoRegistry registry) { this(name, windowSize, null, registry); } - public GlitchFilteredYoInteger(String name, IntegerProvider windowSize, YoInteger position, YoVariableRegistry registry) + public GlitchFilteredYoInteger(String name, IntegerProvider windowSize, YoInteger position, YoRegistry registry) { super(name, GlitchFilteredYoInteger.class.getSimpleName(), registry); @@ -46,11 +46,11 @@ public GlitchFilteredYoInteger(String name, IntegerProvider windowSize, YoIntege } @Override - public void set(int value) + public boolean set(int value) { - super.set(value); if (counter != null) counter.set(0); + return super.set(value); } @Override diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java index 556f17b8..05a6c61e 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java @@ -1,7 +1,7 @@ package us.ihmc.robotics.math.filters; import us.ihmc.commons.MathTools; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -26,24 +26,24 @@ public class JerkLimitedYoVariable extends YoDouble private final YoDouble inputVelocity; private final YoDouble inputAcceleration; - public JerkLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, double dt) + public JerkLimitedYoVariable(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, double dt) { this(name, registry, maxAcceleration, maxJerk, null, null, null, dt); } - public JerkLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, + public JerkLimitedYoVariable(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, YoDouble inputPosition, double dt) { this(name, registry, maxAcceleration, maxJerk, inputPosition, null, null, dt); } - public JerkLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, + public JerkLimitedYoVariable(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, YoDouble inputPosition, YoDouble inputVelocity, double dt) { this(name, registry, maxAcceleration, maxJerk, inputPosition, inputVelocity, null, dt); } - public JerkLimitedYoVariable(String name, YoVariableRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, + public JerkLimitedYoVariable(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, YoDouble inputPosition, YoDouble inputVelocity, YoDouble inputAcceleration, double dt) { super(name, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java index d7ccddce..1c7ec5fb 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java @@ -5,11 +5,11 @@ import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameYawPitchRoll; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFrameYawPitchRoll; public class RateLimitedYoFrameOrientation extends YoFrameYawPitchRoll { @@ -20,39 +20,39 @@ public class RateLimitedYoFrameOrientation extends YoFrameYawPitchRoll private final YoBoolean hasBeenCalled; private final double dt; - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); maxRate.set(initialValue); return maxRate; } - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, YoFrameYawPitchRoll rawOrientation) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawOrientation, rawOrientation.getReferenceFrame()); } - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); } - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, YoFrameYawPitchRoll rawOrientation) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawOrientation, rawOrientation.getReferenceFrame()); } - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); } - private RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + private RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, YoFrameYawPitchRoll rawOrientation, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java index ef258e76..cea995da 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java @@ -4,11 +4,11 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFramePoint3D; public class RateLimitedYoFramePoint extends YoFramePoint3D { @@ -21,38 +21,38 @@ public class RateLimitedYoFramePoint extends YoFramePoint3D private final FrameVector3D differenceVector = new FrameVector3D(); - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); maxRate.set(initialValue); return maxRate; } - public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, FrameTuple3DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); } - public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); } - public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, FrameTuple3DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, rawPosition.getReferenceFrame()); } - public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); } - private RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + private RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java index 109ea90f..b138bf53 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java @@ -5,14 +5,12 @@ import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFramePoint2D; public class RateLimitedYoFramePoint2D extends YoFramePoint2D { @@ -25,38 +23,38 @@ public class RateLimitedYoFramePoint2D extends YoFramePoint2D private final FrameVector2D differenceVector = new FrameVector2D(); - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); maxRate.set(initialValue); return maxRate; } - public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, FrameTuple2DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); } - public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); } - public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, FrameTuple2DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, rawPosition.getReferenceFrame()); } - public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); } - private RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + private RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, FrameTuple2DReadOnly rawPosition, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java index e41da55a..72b6f523 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java @@ -7,7 +7,7 @@ import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameQuaternionBasics; import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; public class RateLimitedYoFramePose implements FixedFramePose3DBasics @@ -15,36 +15,36 @@ public class RateLimitedYoFramePose implements FixedFramePose3DBasics private final RateLimitedYoFramePoint position; private final RateLimitedYoFrameQuaternion orientation; - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); maxRate.set(initialValue); return maxRate; } - public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, FramePose3DReadOnly rawPose) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawPose, rawPose.getReferenceFrame()); } - public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); } - public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, FramePose3DReadOnly rawPose) + public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, FramePose3DReadOnly rawPose) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPose, rawPose.getReferenceFrame()); } - public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); } - private RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + private RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, FramePose3DReadOnly rawPose, ReferenceFrame referenceFrame) { if (rawPose != null) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java index de863547..37a8c317 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java @@ -6,11 +6,11 @@ import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFrameQuaternion; public class RateLimitedYoFrameQuaternion extends YoFrameQuaternion { @@ -21,45 +21,45 @@ public class RateLimitedYoFrameQuaternion extends YoFrameQuaternion private final YoBoolean limited; private final DoubleProvider maxRateVariable; - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); maxRate.set(initialValue); return maxRate; } - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, FrameQuaternionReadOnly rawQuaternion) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawQuaternion.getReferenceFrame(), rawQuaternion); } - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, referenceFrame, null); } - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame, QuaternionReadOnly rawQuaternion) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, referenceFrame, rawQuaternion); } - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, FrameQuaternionReadOnly rawQuaternion) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawQuaternion.getReferenceFrame(), rawQuaternion); } - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, maxRate, dt, referenceFrame, null); } - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, ReferenceFrame referenceFrame, QuaternionReadOnly rawQuaternion) { super(namePrefix, nameSuffix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java index c83c50b6..30dc9e1c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java @@ -4,11 +4,11 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFrameVector3D; public class RateLimitedYoFrameVector extends YoFrameVector3D { @@ -21,38 +21,38 @@ public class RateLimitedYoFrameVector extends YoFrameVector3D private final FrameVector3D differenceVector = new FrameVector3D(); - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); maxRate.set(initialValue); return maxRate; } - public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, FrameTuple3DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); } - public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); } - public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, FrameTuple3DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, rawPosition.getReferenceFrame()); } - public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); } - private RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + private RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, referenceFrame, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java index 4afeb977..2c32bbbb 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java @@ -4,11 +4,11 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoFrameVector2D; public class RateLimitedYoFrameVector2d extends YoFrameVector2D @@ -23,22 +23,22 @@ private RateLimitedYoFrameVector2d(RateLimitedYoVariable x, RateLimitedYoVariabl this.y = y; } - public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { - RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); - RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); + RateLimitedYoVariable x = new RateLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); + RateLimitedYoVariable y = new RateLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, referenceFrame); return ret; } - public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, YoDouble maxRate, double dt, ReferenceFrame referenceFrame) { - RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); - RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); + RateLimitedYoVariable x = new RateLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); + RateLimitedYoVariable y = new RateLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, referenceFrame); @@ -46,11 +46,11 @@ public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String } - public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, YoFrameVector2D unfilteredVector) { - RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); - RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); + RateLimitedYoVariable x = new RateLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); + RateLimitedYoVariable y = new RateLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); @@ -58,11 +58,11 @@ public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String } - public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, YoFrameVector2D unfilteredVector) { - RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); - RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); + RateLimitedYoVariable x = new RateLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); + RateLimitedYoVariable y = new RateLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java index 3c794ee6..02871c05 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java @@ -4,11 +4,11 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoMutableFrameVector3D; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.frameObjects.YoMutableFrameVector3D; public class RateLimitedYoMutableFrameVector3D extends YoMutableFrameVector3D { @@ -21,39 +21,39 @@ public class RateLimitedYoMutableFrameVector3D extends YoMutableFrameVector3D private final FrameVector3D differenceVector = new FrameVector3D(); - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoVariableRegistry registry) + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) { YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); maxRate.set(initialValue); return maxRate; } - public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, FrameTuple3DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); } - public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); } - public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, FrameTuple3DReadOnly rawPosition) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, rawPosition.getReferenceFrame()); } - public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, + public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); } - private RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maxRate, double dt, + private RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) { super(namePrefix, nameSuffix, registry, referenceFrame); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java index 3d889b19..60705109 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java @@ -1,7 +1,7 @@ package us.ihmc.robotics.math.filters; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -16,7 +16,7 @@ public class RateLimitedYoVariable extends YoDouble private final YoBoolean hasBeenCalled; - public RateLimitedYoVariable(String name, YoVariableRegistry registry, double maxRate, double dt) + public RateLimitedYoVariable(String name, YoRegistry registry, double maxRate, double dt) { super(name, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); @@ -33,7 +33,7 @@ public RateLimitedYoVariable(String name, YoVariableRegistry registry, double ma reset(); } - public RateLimitedYoVariable(String name, YoVariableRegistry registry, DoubleProvider maxRateVariable, double dt) + public RateLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRateVariable, double dt) { super(name, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); @@ -46,7 +46,7 @@ public RateLimitedYoVariable(String name, YoVariableRegistry registry, DoublePro reset(); } - public RateLimitedYoVariable(String name, YoVariableRegistry registry, double maxRate, YoDouble positionVariable, double dt) + public RateLimitedYoVariable(String name, YoRegistry registry, double maxRate, YoDouble positionVariable, double dt) { super(name, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); @@ -63,7 +63,7 @@ public RateLimitedYoVariable(String name, YoVariableRegistry registry, double ma reset(); } - public RateLimitedYoVariable(String name, YoVariableRegistry registry, DoubleProvider maxRateVariable, YoDouble positionVariable, double dt) + public RateLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRateVariable, YoDouble positionVariable, double dt) { super(name, registry); this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java index 40f7c194..8fc8a2a4 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java @@ -2,7 +2,7 @@ import us.ihmc.commons.MathTools; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoEnum; @@ -31,7 +31,7 @@ public class RevisedBacklashCompensatingVelocityYoVariable extends YoDouble impl private final YoDouble timeSinceSloppy; public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, YoDouble positionVariable, - double dt, DoubleProvider slopTime, YoVariableRegistry registry) + double dt, DoubleProvider slopTime, YoRegistry registry) { super(name, description, registry); @@ -57,7 +57,7 @@ public RevisedBacklashCompensatingVelocityYoVariable(String name, String descrip } public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, double dt, YoDouble slopTime, - YoVariableRegistry registry) + YoRegistry registry) { super(name, description, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java index ebdf4d31..7b328c2d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java @@ -3,9 +3,9 @@ import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.robotics.math.frames.YoFrameVariableNameTools; -import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoFrameVector3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; public class SecondOrderFilteredYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable { @@ -21,7 +21,7 @@ private SecondOrderFilteredYoFrameVector(SecondOrderFilteredYoVariable x, Second this.z = z; } - public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType, ReferenceFrame referenceFrame) { SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, registry, naturalFrequencyInHz, @@ -29,17 +29,17 @@ public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameV return createSecondOrderFilteredYoFrameVector(namePrefix, nameSuffix, registry, dt, parameters, referenceFrame); } - public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double dt, SecondOrderFilteredYoVariableParameters parameters, ReferenceFrame referenceFrame) { SecondOrderFilteredYoVariable x, y, z; - x = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, dt, parameters); - y = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, dt, parameters); - z = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, dt, parameters); + x = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, dt, parameters); + y = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, dt, parameters); + z = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), registry, dt, parameters); return new SecondOrderFilteredYoFrameVector(x, y, z, referenceFrame); } - public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType, YoFrameVector3D unfilteredVector) { SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, registry, naturalFrequencyInHz, @@ -47,13 +47,13 @@ public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameV return createSecondOrderFilteredYoFrameVector(namePrefix, nameSuffix, registry, dt, parameters, unfilteredVector); } - public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, + public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double dt, SecondOrderFilteredYoVariableParameters parameters, YoFrameVector3D unfilteredVector) { SecondOrderFilteredYoVariable x, y, z; - x = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoX()); - y = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoY()); - z = new SecondOrderFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoZ()); + x = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoX()); + y = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoY()); + z = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoZ()); return new SecondOrderFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java index 8c3cac60..d40f88c9 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; public class SecondOrderFilteredYoVariableParameters @@ -9,7 +9,7 @@ public class SecondOrderFilteredYoVariableParameters private final YoDouble dampingRatio; private final SecondOrderFilterType filterType; - public SecondOrderFilteredYoVariableParameters(String name, YoVariableRegistry registry, double naturalFrequencyInHz, double dampingRatio, + public SecondOrderFilteredYoVariableParameters(String name, YoRegistry registry, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType) { this.naturalFrequencyInHz = new YoDouble(name + "NaturalFrequency", registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java index 087ad981..efe57e2a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java @@ -1,14 +1,11 @@ package us.ihmc.robotics.math.filters; -import static us.ihmc.robotics.math.frames.YoFrameVariableNameTools.createXName; -import static us.ihmc.robotics.math.frames.YoFrameVariableNameTools.createYName; -import static us.ihmc.robotics.math.frames.YoFrameVariableNameTools.createZName; - import us.ihmc.euclid.referenceFrame.FrameVector3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoFrameVector3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; public class SimpleMovingAverageFilteredYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable { @@ -25,11 +22,11 @@ private SimpleMovingAverageFilteredYoFrameVector(SimpleMovingAverageFilteredYoVa } public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, - ReferenceFrame referenceFrame, YoVariableRegistry registry) + ReferenceFrame referenceFrame, YoRegistry registry) { - String xName = createXName(namePrefix, nameSuffix); - String yName = createYName(namePrefix, nameSuffix); - String zName = createZName(namePrefix, nameSuffix); + String xName = YoGeometryNameTools.createXName(namePrefix, nameSuffix); + String yName = YoGeometryNameTools.createYName(namePrefix, nameSuffix); + String zName = YoGeometryNameTools.createZName(namePrefix, nameSuffix); SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, registry); SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, registry); @@ -39,11 +36,11 @@ public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverage } public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, - YoFrameVector3D unfilteredVector, YoVariableRegistry registry) + YoFrameVector3D unfilteredVector, YoRegistry registry) { - String xName = createXName(namePrefix, nameSuffix); - String yName = createYName(namePrefix, nameSuffix); - String zName = createZName(namePrefix, nameSuffix); + String xName = YoGeometryNameTools.createXName(namePrefix, nameSuffix); + String yName = YoGeometryNameTools.createYName(namePrefix, nameSuffix); + String zName = YoGeometryNameTools.createZName(namePrefix, nameSuffix); SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, unfilteredVector.getYoX(), registry); SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, unfilteredVector.getYoY(), registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java index 17d7cfef..f3945330 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java @@ -3,7 +3,7 @@ import org.ejml.data.DMatrixRMaj; import org.ejml.dense.row.CommonOps_DDRM; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoInteger; @@ -20,12 +20,12 @@ public class SimpleMovingAverageFilteredYoVariable extends YoDouble private boolean bufferHasBeenFilled = false; - public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoVariableRegistry registry) + public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoRegistry registry) { this(name, windowSize, null, registry); } - public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoDouble yoVariableToFilter, YoVariableRegistry registry) + public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoDouble yoVariableToFilter, YoRegistry registry) { super(name, registry); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java index 9d9e08b8..f42d60a8 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java @@ -2,7 +2,7 @@ import org.ejml.data.DMatrixRMaj; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoInteger; @@ -21,7 +21,7 @@ public class YoMatrix private final YoInteger numberOfRows, numberOfColumns; private final YoDouble[][] variables; - public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, YoVariableRegistry registry) + public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) { this.maxNumberOfRows = maxNumberOfRows; this.maxNumberOfColumns = maxNumberOfColumns; From 4cfe0e5b47717a23ef6f4f39190ae281c9b184bb Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Tue, 6 Jul 2021 17:09:05 -0500 Subject: [PATCH 22/47] Cleaned up the Butterworth filter. --- .../us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java index 9477e7ed..6da82cc5 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java @@ -154,7 +154,7 @@ public static double computeAlphaGivenBreakFrequencyProperly(double breakFrequen public static double computeBreakFrequencyGivenAlpha(double alpha, double dt) { - return (1.0 - alpha) / (Math.PI * dt + Math.PI * alpha * dt); + return (1.0 - alpha) / (Math.PI * dt * (1.0 + alpha)); } public static void main(String[] args) From 1abfce0816c051c40cdd2cf0e85ed0318bc89b3e Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Fri, 1 Jul 2022 07:59:32 -0500 Subject: [PATCH 23/47] :arrow_up: euclid 0.18.1 --- .../us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java index 79f84837..f9523e64 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java @@ -2,6 +2,7 @@ import org.apache.commons.lang3.NotImplementedException; +import us.ihmc.euclid.interfaces.EuclidGeometry; import us.ihmc.euclid.transform.interfaces.Transform; import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics; import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; @@ -94,4 +95,9 @@ public void applyInverseTransform(Transform transform, boolean checkIfTransformI throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + "."); } + @Override + public boolean geometricallyEquals(EuclidGeometry geometry, double epsilon) + { + return epsilonEquals(geometry, epsilon); + } } From 01e2f36e34ba01328a1c5299416716077f5f66bd Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Tue, 18 Oct 2022 22:53:08 -0500 Subject: [PATCH 24/47] fixed a comment --- .../robotics/math/filters/FilteredVelocityYoFrameVector.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java index 369c49bc..334fcd9e 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java @@ -22,6 +22,10 @@ * IHMC *

* + * 
+ *            vel_{n} = alpha * vel{n-1} + (1 - alpha) * (pos_{n} - pos_{n-1})
+ *
+ * * @author IHMC Biped Team * @version 1.0 */ From f4333a95b1ec40a153653776607fdbc9787e7c00 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Mon, 3 Oct 2022 16:28:28 -0500 Subject: [PATCH 25/47] Cleaned unused AverageYoDouble and AverageYoFrameVector3D. Made the reset consistent with other filtering classes --- .../math/filters/AverageYoDouble.java | 52 +++++++++++++ .../math/filters/AverageYoFrameVector3D.java | 78 +++++++++++++++++++ 2 files changed, 130 insertions(+) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoDouble.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoFrameVector3D.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoDouble.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoDouble.java new file mode 100644 index 00000000..d42f5774 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoDouble.java @@ -0,0 +1,52 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoInteger; + +public class AverageYoDouble extends YoDouble +{ + private final YoInteger sampleSize; + private final DoubleProvider dataSource; + + public AverageYoDouble(String name, YoRegistry registry) + { + this(name, null, registry); + } + + public AverageYoDouble(String name, DoubleProvider dataSource, YoRegistry registry) + { + super(name, registry); + + this.dataSource = dataSource; + sampleSize = new YoInteger(name + "SampleSize", registry); + } + + public void update() + { + if (dataSource == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "dataSource variable to call update(), otherwise use update(double)"); + } + + update(dataSource.getValue()); + } + + public void update(double dataSource) + { + sampleSize.increment(); + add((dataSource - getValue()) / sampleSize.getValue()); + } + + public void reset() + { + sampleSize.set(0); + } + + public int getSampleSize() + { + return sampleSize.getValue(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoFrameVector3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoFrameVector3D.java new file mode 100644 index 00000000..d7197689 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoFrameVector3D.java @@ -0,0 +1,78 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; +import us.ihmc.yoVariables.variable.YoInteger; + +public class AverageYoFrameVector3D extends YoFrameVector3D +{ + private final YoInteger sampleSize; + private final Tuple3DReadOnly dataSource; + + public AverageYoFrameVector3D(String namePrefix, ReferenceFrame referenceFrame, YoRegistry registry) + { + this(namePrefix, "", registry, referenceFrame, null); + } + + public AverageYoFrameVector3D(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, YoRegistry registry) + { + this(namePrefix, nameSuffix, registry, referenceFrame, null); + } + + public AverageYoFrameVector3D(String namePrefix, YoRegistry registry, FrameTuple3DReadOnly dataSource) + { + this(namePrefix, "", registry, dataSource.getReferenceFrame(), dataSource); + } + + public AverageYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, FrameTuple3DReadOnly dataSource) + { + this(namePrefix, nameSuffix, registry, dataSource.getReferenceFrame(), dataSource); + } + + private AverageYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, ReferenceFrame referenceFrame, Tuple3DReadOnly dataSource) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + this.dataSource = dataSource; + + sampleSize = new YoInteger(YoGeometryNameTools.assembleName(namePrefix, "sampleSize", nameSuffix), registry); + } + + public void update() + { + update(dataSource); + } + + public void update(Tuple3DReadOnly vectorSource) + { + update(vectorSource.getX(), vectorSource.getY(), vectorSource.getZ()); + } + + public void update(FrameTuple3DReadOnly vectorSource) + { + checkReferenceFrameMatch(vectorSource); + update((Tuple3DReadOnly) vectorSource); + } + + public void update(double xSource, double ySource, double zSource) + { + sampleSize.increment(); + setX(getX() + (xSource - getX()) / sampleSize.getValue()); + setY(getY() + (ySource - getY()) / sampleSize.getValue()); + setZ(getZ() + (zSource - getZ()) / sampleSize.getValue()); + } + + public void reset() + { + sampleSize.set(0); + } + + public int getSampleSize() + { + return sampleSize.getValue(); + } +} From 8005901a2e406d75db7a8ba0966bfa94b5f0092e Mon Sep 17 00:00:00 2001 From: Operator Computer Date: Wed, 5 Apr 2023 17:14:56 -0500 Subject: [PATCH 26/47] Bugfixes for the IK streaming --- .../filters/AlphaFilteredYoFramePose3D.java | 106 ++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java new file mode 100644 index 00000000..597c35c7 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java @@ -0,0 +1,106 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.geometry.Pose3D; +import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePose3D; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class AlphaFilteredYoFramePose3D extends YoFramePose3D implements ProcessingYoVariable +{ + private final DoubleProvider alpha; + private final FramePose3DReadOnly unfilteredPose; + private final YoBoolean hasBeenCalled; + + private final Pose3D poseMeasured = new Pose3D(); + private final Pose3D posePreviousFiltered = new Pose3D(); + + private static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "AlphaVariable", registry); + maxRate.set(initialValue); + return maxRate; + } + + public AlphaFilteredYoFramePose3D(String namePrefix, String nameSuffix, FramePose3DReadOnly unfilteredPose, double alpha, YoRegistry registry) + { + this(namePrefix, nameSuffix, unfilteredPose, createAlphaYoDouble(namePrefix, alpha, registry), registry); + } + + public AlphaFilteredYoFramePose3D(String namePrefix, String nameSuffix, DoubleProvider alpha, ReferenceFrame referenceFrame, YoRegistry registry) + { + this(namePrefix, nameSuffix, null, alpha, referenceFrame, registry); + } + + public AlphaFilteredYoFramePose3D(String namePrefix, String nameSuffix, FramePose3DReadOnly unfilteredPose, DoubleProvider alpha, YoRegistry registry) + { + this(namePrefix, nameSuffix, unfilteredPose, alpha, unfilteredPose.getReferenceFrame(), registry); + } + + private AlphaFilteredYoFramePose3D(String namePrefix, + String nameSuffix, + FramePose3DReadOnly unfilteredPose, + DoubleProvider alpha, + ReferenceFrame referenceFrame, + YoRegistry registry) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + this.unfilteredPose = unfilteredPose; + + if (alpha == null) + alpha = createAlphaYoDouble(namePrefix, 0.0, registry); + this.alpha = alpha; + + this.hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); + } + + @Override + public void update() + { + if (unfilteredPose == null) + { + throw new NullPointerException("AlphaFilteredYoFramePose3D must be constructed with a non null " + + "pose variable to call update(), otherwise use update(Pose3DReadOnly)"); + } + + poseMeasured.set(unfilteredPose); + update(poseMeasured); + } + + public void update(FramePose3DReadOnly rawPose) + { + checkReferenceFrameMatch(rawPose); + poseMeasured.set(rawPose); + update(poseMeasured); + } + + public void update(Pose3DReadOnly rawPose) + { + if (hasBeenCalled.getBooleanValue()) + { + posePreviousFiltered.set(this); + + interpolate(poseMeasured, posePreviousFiltered, alpha.getValue()); // qPreviousFiltered 'gets multiplied by alpha' + } + else + { + set(poseMeasured); + hasBeenCalled.set(true); + } + } + + @Override + public void reset() + { + hasBeenCalled.set(false); + } + + public FramePose3DReadOnly getUnfilteredPose() + { + return unfilteredPose; + } +} From 83f3cf712a0af4ba321c1faa153ee6b7801d0d95 Mon Sep 17 00:00:00 2001 From: Duncan Calvert Date: Wed, 2 Aug 2023 18:23:27 -0500 Subject: [PATCH 27/47] Implement ArUco marker alpha filtering. Simplify from Dex's feedback. Get filtering with break frequency working. Add and test reset. Call reset. --- .../AlphaFilteredRigidBodyTransform.java | 42 +++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java new file mode 100644 index 00000000..ded68f13 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java @@ -0,0 +1,42 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.transform.RigidBodyTransform; + +public class AlphaFilteredRigidBodyTransform extends RigidBodyTransform +{ + private double alpha = 0.0; + private final RigidBodyTransform previousFiltered = new RigidBodyTransform(); + + public AlphaFilteredRigidBodyTransform() + { + reset(); + } + + public void update(RigidBodyTransform measured) + { + if (containsNaN()) + { + set(measured); + } + else + { + previousFiltered.set(this); + interpolate(measured, previousFiltered, alpha); + } + } + + public double getAlpha() + { + return alpha; + } + + public void setAlpha(double alpha) + { + this.alpha = alpha; + } + + public void reset() + { + setToNaN(); + } +} From f7d502b159da4e16322062602b62e5a34fa1cc16 Mon Sep 17 00:00:00 2001 From: Nadia Operator Date: Thu, 3 Aug 2023 18:23:41 -0500 Subject: [PATCH 28/47] added some yo variables for com velocity finally, and fixed some naming in the joint torque based foot switch --- .../math/filters/GlitchFilteredYoBoolean.java | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java index 0f85d3cf..530dab44 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java @@ -42,13 +42,25 @@ public GlitchFilteredYoBoolean(String name, YoRegistry registry, YoBoolean yoVar this(name, "", registry, yoVariableToFilter, windowSize); } + public GlitchFilteredYoBoolean(String name, YoRegistry registry, YoBoolean yoVariableToFilter, YoInteger windowSize) + { + this(name, "", registry, yoVariableToFilter, windowSize); + } + public GlitchFilteredYoBoolean(String name, String description, YoRegistry registry, YoBoolean yoVariableToFilter, int windowSize) + { + this(name, description, registry, yoVariableToFilter, new YoInteger(name + "WindowSize", description, registry)); + + this.windowSize.set(windowSize); + } + + public GlitchFilteredYoBoolean(String name, String description, YoRegistry registry, YoBoolean yoVariableToFilter, YoInteger windowSize) { super(name, description, registry); counter = new YoInteger(name + "Count", description, registry); - this.windowSize = new YoInteger(name + "WindowSize", description, registry); + this.windowSize = windowSize; - initialize(yoVariableToFilter, windowSize); + initialize(yoVariableToFilter, windowSize.getIntegerValue()); } private void initialize(YoBoolean yoVariableToFilter, int windowSize) From 43d1a9cc3e4b55eb25ed0a8ff8380aab137b4f1b Mon Sep 17 00:00:00 2001 From: James Foster Date: Mon, 4 Dec 2023 17:45:33 -0600 Subject: [PATCH 29/47] Add constructors to YoMatrix that allow one to name both rows and columns with String arrays Improved the API Fixed silly constructor logic error Fix bug of exception never being thrown Temporary port of in-progress YoMatrix changes --- .../ihmc/robotics/math/frames/YoMatrix.java | 261 ++++++++++++++---- 1 file changed, 214 insertions(+), 47 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java index f42d60a8..8585174b 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java @@ -1,7 +1,8 @@ package us.ihmc.robotics.math.frames; -import org.ejml.data.DMatrixRMaj; +import org.ejml.data.*; +import org.ejml.ops.MatrixIO; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoInteger; @@ -14,14 +15,43 @@ * * @author JerryPratt */ -public class YoMatrix +public class YoMatrix implements DMatrix, ReshapeMatrix { + // TODO: eventually consolidate YoMatrix implementations + + private static final long serialVersionUID = 2156411740647948028L; + private final int maxNumberOfRows, maxNumberOfColumns; private final YoInteger numberOfRows, numberOfColumns; private final YoDouble[][] variables; public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) + { + this(name, null, maxNumberOfRows, maxNumberOfColumns, null, null, registry); + } + + public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, YoRegistry registry) + { + this(name, null, maxNumberOfRows, maxNumberOfColumns, rowNames, null, registry); + } + + public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) + { + this(name, null, maxNumberOfRows, maxNumberOfColumns, rowNames, columnNames, registry); + } + + public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) + { + this(name, description, maxNumberOfRows, maxNumberOfColumns, null, null, registry); + } + + public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, YoRegistry registry) + { + this(name, description, maxNumberOfRows, maxNumberOfColumns, rowNames, null, registry); + } + + public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) { this.maxNumberOfRows = maxNumberOfRows; this.maxNumberOfColumns = maxNumberOfColumns; @@ -38,7 +68,183 @@ public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, YoRegi { for (int column = 0; column < maxNumberOfColumns; column++) { - variables[row][column] = new YoDouble(name + "_" + row + "_" + column, registry); + switch (checkNames(rowNames, columnNames)) + { + case NONE: + { + variables[row][column] = new YoDouble(name + "_" + row + "_" + column, description, registry); // names are simply the row and column indices + break; + } + case ROWS: + { + if (maxNumberOfColumns > 1) + throw new IllegalArgumentException("The YoMatrix must be a column vector if only row names are provided, else unique names cannot be generated."); + + variables[row][column] = new YoDouble(name + "_" + rowNames[row], description, registry); // names are the row names, no column identifier + break; + } + case ROWS_AND_COLUMNS: + { + variables[row][column] = new YoDouble(name + "_" + rowNames[row] + "_" + columnNames[column], description, registry); // names are the row and column names + break; + } + } + } + } + } + + private enum NamesProvided + { + NONE, ROWS, ROWS_AND_COLUMNS + } + + private NamesProvided checkNames(String[] rowNames, String[] columnNames) + { + if (rowNames == null && columnNames == null) + return NamesProvided.NONE; + else if (rowNames != null && columnNames == null) + return NamesProvided.ROWS; + else + return NamesProvided.ROWS_AND_COLUMNS; + } + + @Override + public double get(int row, int col) + { + if (col < 0 || col >= getNumCols() || row < 0 || row >= getNumRows()) + throw new IllegalArgumentException("Specified element is out of bounds: (" + row + " , " + col + ")"); + return unsafe_get(row, col); + } + + @Override + public double unsafe_get(int row, int col) + { + return variables[row][col].getValue(); + } + + @Override + public void set(int row, int col, double val) + { + if (col < 0 || col >= getNumCols() || row < 0 || row >= getNumRows()) + throw new IllegalArgumentException("Specified element is out of bounds: (" + row + " , " + col + ")"); + unsafe_set(row, col, val); + } + + @Override + public void unsafe_set(int row, int col, double val) + { + variables[row][col].set(val); + } + + @Override + public int getNumElements() + { + return numberOfRows.getValue() * numberOfColumns.getValue(); + } + + @Override + public int getNumRows() + { + return numberOfRows.getValue(); + } + + @Override + public int getNumCols() + { + return numberOfColumns.getValue(); + } + + @Override + public void zero() + { + for (int row = 0; row < getNumRows(); row++) + { + for (int col = 0; col < getNumCols(); col++) + { + variables[row][col].set(0.0); + } + } + } + + @Override + public T copy() + { + throw new UnsupportedOperationException(); + } + + @Override + public T createLike() + { + throw new UnsupportedOperationException(); + } + + @Override + public T create(int numRows, int numCols) + { + throw new UnsupportedOperationException(); + } + + @Override + public void set(Matrix original) + { + if (original instanceof DMatrix) + { + DMatrix otherMatrix = (DMatrix) original; + reshape(otherMatrix.getNumRows(), otherMatrix.getNumRows()); + for (int row = 0; row < getNumRows(); row++) + { + for (int col = 0; col < getNumCols(); col++) + { + set(row, col, otherMatrix.unsafe_get(row, col)); + } + } + } + } + + @Override + public void print() + { + MatrixIO.printFancy(System.out, this, MatrixIO.DEFAULT_LENGTH); + } + + @Override + public void print(String format) + { + MatrixIO.print(System.out, this, format); + } + + @Override + public MatrixType getType() + { + return MatrixType.UNSPECIFIED; + } + + @Override + public void reshape(int numRows, int numCols) + { + if (numRows > maxNumberOfRows) + throw new IllegalArgumentException("Too many rows. Expected less or equal to " + maxNumberOfRows + ", was " + numRows); + else if (numCols > maxNumberOfColumns) + throw new IllegalArgumentException("Too many columns. Expected less or equal to " + maxNumberOfColumns + ", was " + numCols); + else if (numRows < 0 || numCols < 0) + throw new IllegalArgumentException("Cannot reshape with a negative number of rows or columns."); + + numberOfRows.set(numRows); + numberOfColumns.set(numCols); + + for (int row = 0; row < numRows; row++) + { + for (int col = numCols; col < maxNumberOfColumns; col++) + { + unsafe_set(row, col, Double.NaN); + } + } + + for (int row = numRows; row < maxNumberOfRows; row++) + { + for (int col = 0; col < maxNumberOfColumns; col++) + { + unsafe_set(row, col, Double.NaN); } } } @@ -72,7 +278,7 @@ public void set(DMatrixRMaj matrix) public void getAndReshape(DMatrixRMaj matrixToPack) { - matrixToPack.reshape(getNumberOfRows(), getNumberOfColumns()); + matrixToPack.reshape(getNumRows(), getNumCols()); get(matrixToPack); } @@ -95,53 +301,14 @@ public void get(DMatrixRMaj matrixToPack) } } - public int getNumberOfRows() - { - return numberOfRows.getIntegerValue(); - } - - public int getNumberOfColumns() - { - return numberOfColumns.getIntegerValue(); - } - - public void setToZero(int numberOfRows, int numberOfColumns) - { - if (((numberOfRows > maxNumberOfRows) || (numberOfColumns > maxNumberOfColumns)) && (numberOfRows > 0) && (numberOfColumns > 0)) - throw new RuntimeException("Not enough rows or columns: " + numberOfRows + " by " + numberOfColumns); - - this.numberOfRows.set(numberOfRows); - this.numberOfColumns.set(numberOfColumns); - - for (int row = 0; row < maxNumberOfRows; row++) - { - for (int column = 0; column < maxNumberOfColumns; column++) - { - if ((row < numberOfRows) && (column < numberOfColumns)) - { - variables[row][column].set(0.0); - } - else - { - variables[row][column].set(Double.NaN); - } - } - } - } - public void setToNaN(int numberOfRows, int numberOfColumns) { - if (((numberOfRows > maxNumberOfRows) || (numberOfColumns > maxNumberOfColumns)) && (numberOfRows > 0) && (numberOfColumns > 0)) - throw new RuntimeException("Not enough rows or columns: " + numberOfRows + " by " + numberOfColumns); - - this.numberOfRows.set(numberOfRows); - this.numberOfColumns.set(numberOfColumns); - - for (int row = 0; row < maxNumberOfRows; row++) + reshape(numberOfRows, numberOfColumns); + for (int row = 0; row < numberOfRows; row++) { - for (int column = 0; column < maxNumberOfColumns; column++) + for (int col = 0; col < numberOfColumns; col++) { - variables[row][column].set(Double.NaN); + unsafe_set(row, col, Double.NaN); } } } From a2ff33756586438f1b54e09549d48cc82843a456 Mon Sep 17 00:00:00 2001 From: James Foster Date: Wed, 31 Jan 2024 14:17:38 -0600 Subject: [PATCH 30/47] Introduced alpha-filtered version of estimate from KF Inconsistent constructor naming Make YoMatrix set and get API more general More correct implementation of AlphaFilteredYoMatrix, more API for setting and solving separately Fix bug in YoMatrix Remove underscore seperators from YoMatrix Refactored the updating of actual robot spatial inertias --- .../math/filters/AlphaFilteredYoMatrix.java | 111 ++++++++++++++++++ .../ihmc/robotics/math/frames/YoMatrix.java | 26 ++-- 2 files changed, 126 insertions(+), 11 deletions(-) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java new file mode 100644 index 00000000..50949bf9 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java @@ -0,0 +1,111 @@ +package us.ihmc.robotics.math.filters; + +import org.ejml.data.DMatrix; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import us.ihmc.robotics.math.frames.YoMatrix; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class AlphaFilteredYoMatrix extends YoMatrix +{ + private final DMatrixRMaj previous; + private final DMatrixRMaj current; + private final DMatrixRMaj filtered; + + private final YoDouble alpha; + + public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, YoRegistry registry) + { + this(name, null, alpha, numberOfRows, numberOfColumns, null, null, registry); + } + + public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, YoRegistry registry) + { + this(name, null, alpha, numberOfRows, numberOfColumns, rowNames, null, registry); + } + + public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) + { + this(name, null, alpha, numberOfRows, numberOfColumns, rowNames, columnNames, registry); + } + + public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, YoRegistry registry) + { + this(name, description, alpha, numberOfRows, numberOfColumns, null, null, registry); + } + + public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, YoRegistry registry) + { + this(name, description, alpha, numberOfRows, numberOfColumns, rowNames, null, registry); + } + + public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) + { + super(name, description, numberOfRows, numberOfColumns, rowNames, columnNames, registry); + this.alpha = new YoDouble(name + "_alpha", registry); + this.alpha.set(alpha); + + previous = new DMatrixRMaj(numberOfRows, numberOfColumns); + current = new DMatrixRMaj(numberOfRows, numberOfColumns); + filtered = new DMatrixRMaj(numberOfRows, numberOfColumns); + } + + public void setAlpha(double alpha) + { + this.alpha.set(alpha); + } + + /** + * Set the current value of the matrix to be filtered. + *

+ * NOTE: This method does not solve for the filtered value. To solve for the filtered value, use {@link #solve()}. + *

+ * + * @param current the current value of the matrix to be filtered. Not modified. + */ + @Override + public void set(DMatrix current) + { + super.set(current); + this.current.set(current); + } + + /** + * Assuming that the current value has been set, this method solves for the filtered value. + *

+ * See {@link #set(DMatrix)} for how to set the matrix's current value. + *

+ */ + public void solve() + { + filtered.set(previous); + CommonOps_DDRM.scale(alpha.getDoubleValue(), filtered); + + super.get(current); + CommonOps_DDRM.addEquals(filtered, 1 - alpha.getDoubleValue(), current); + + // Set the previous value to be the output of the filter, so it can be used next time + previous.set(filtered); + super.set(filtered); + } + + /** + * Set the current value of the matrix to be filtered and solve for the filtered value. + * + * @param current the current value of the matrix to be filtered. Not modified. + */ + public void setAndSolve(DMatrix current) + { + filtered.set(previous); + CommonOps_DDRM.scale(alpha.getDoubleValue(), filtered); + + super.set(current); + this.current.set(current); + CommonOps_DDRM.addEquals(filtered, 1 - alpha.getDoubleValue(), this.current); + + // Set the previous value to be the output of the filter, so it can be used next time + previous.set(filtered); + super.set(filtered); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java index 8585174b..1ebf58e5 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java @@ -1,7 +1,6 @@ package us.ihmc.robotics.math.frames; import org.ejml.data.*; - import org.ejml.ops.MatrixIO; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; @@ -12,7 +11,7 @@ * maximum number of rows and columns and an actual number of rows and columns. If you set with a * smaller matrix, then the actual size will be the size of the passed in matrix. extra entries will * be set to NaN. If you get the contents the matrix you pack must be the correct size. - * + * * @author JerryPratt */ public class YoMatrix implements DMatrix, ReshapeMatrix @@ -72,20 +71,21 @@ public YoMatrix(String name, String description, int maxNumberOfRows, int maxNum { case NONE: { - variables[row][column] = new YoDouble(name + "_" + row + "_" + column, description, registry); // names are simply the row and column indices + variables[row][column] = new YoDouble(name + row + column, description, registry); // names are simply the row and column indices break; } case ROWS: { if (maxNumberOfColumns > 1) - throw new IllegalArgumentException("The YoMatrix must be a column vector if only row names are provided, else unique names cannot be generated."); + throw new IllegalArgumentException( + "The YoMatrix must be a column vector if only row names are provided, else unique names cannot be generated."); - variables[row][column] = new YoDouble(name + "_" + rowNames[row], description, registry); // names are the row names, no column identifier + variables[row][column] = new YoDouble(name + rowNames[row], description, registry); // names are the row names, no column identifier break; } case ROWS_AND_COLUMNS: { - variables[row][column] = new YoDouble(name + "_" + rowNames[row] + "_" + columnNames[column], description, registry); // names are the row and column names + variables[row][column] = new YoDouble(name + rowNames[row] + columnNames[column], description, registry); // names are the row and column names break; } } @@ -187,10 +187,9 @@ public T create(int numRows, int numCols) @Override public void set(Matrix original) { - if (original instanceof DMatrix) + if (original instanceof DMatrix otherMatrix) { - DMatrix otherMatrix = (DMatrix) original; - reshape(otherMatrix.getNumRows(), otherMatrix.getNumRows()); + reshape(otherMatrix.getNumRows(), otherMatrix.getNumCols()); for (int row = 0; row < getNumRows(); row++) { for (int col = 0; col < getNumCols(); col++) @@ -249,7 +248,7 @@ else if (numRows < 0 || numCols < 0) } } - public void set(DMatrixRMaj matrix) + public void set(DMatrix matrix) { int numRows = matrix.getNumRows(); int numCols = matrix.getNumCols(); @@ -282,7 +281,7 @@ public void getAndReshape(DMatrixRMaj matrixToPack) get(matrixToPack); } - public void get(DMatrixRMaj matrixToPack) + public void get(DMatrix matrixToPack) { int numRows = matrixToPack.getNumRows(); int numCols = matrixToPack.getNumCols(); @@ -312,4 +311,9 @@ public void setToNaN(int numberOfRows, int numberOfColumns) } } } + + public YoDouble getYoDouble(int row, int col) + { + return variables[row][col]; + } } From 9ff8edb0507e06bd93598cebec4e1505ba5ce783 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Thu, 22 Feb 2024 17:33:28 -0600 Subject: [PATCH 31/47] More cleanup of the state estimator. --- .../filters/AlphaFilteredYoFramePose3D.java | 26 +++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java index 597c35c7..61171278 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java @@ -4,9 +4,12 @@ import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePose3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; @@ -58,13 +61,32 @@ private AlphaFilteredYoFramePose3D(String namePrefix, this.hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); } + public AlphaFilteredYoFramePose3D(YoFramePoint3D position, + YoFrameQuaternion orientation, + FramePose3DReadOnly unfilteredPose, + DoubleProvider alpha, + YoRegistry registry) + { + super(position, orientation); + this.unfilteredPose = unfilteredPose; + + String namePrefix = YoGeometryNameTools.getCommonPrefix(position.getNamePrefix(), orientation.getNamePrefix()); + String nameSuffix = YoGeometryNameTools.getCommonSuffix(position.getNameSuffix(), orientation.getNameSuffix()); + + if (alpha == null) + alpha = createAlphaYoDouble(namePrefix, 0.0, registry); + this.alpha = alpha; + + this.hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); + } + @Override public void update() { if (unfilteredPose == null) { - throw new NullPointerException("AlphaFilteredYoFramePose3D must be constructed with a non null " - + "pose variable to call update(), otherwise use update(Pose3DReadOnly)"); + throw new NullPointerException( + "AlphaFilteredYoFramePose3D must be constructed with a non null " + "pose variable to call update(), otherwise use update(Pose3DReadOnly)"); } poseMeasured.set(unfilteredPose); From e0971cc92c037d6cb3492eadd0e8f8fd45eb714d Mon Sep 17 00:00:00 2001 From: James Foster Date: Fri, 29 Mar 2024 15:34:42 -0500 Subject: [PATCH 32/47] Simplifications to AlphaFilteredYoMatrix --- .../ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java index 50949bf9..296f315c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java @@ -79,8 +79,7 @@ public void set(DMatrix current) */ public void solve() { - filtered.set(previous); - CommonOps_DDRM.scale(alpha.getDoubleValue(), filtered); + CommonOps_DDRM.scale(alpha.getDoubleValue(), previous, filtered); super.get(current); CommonOps_DDRM.addEquals(filtered, 1 - alpha.getDoubleValue(), current); @@ -97,8 +96,7 @@ public void solve() */ public void setAndSolve(DMatrix current) { - filtered.set(previous); - CommonOps_DDRM.scale(alpha.getDoubleValue(), filtered); + CommonOps_DDRM.scale(alpha.getDoubleValue(), previous, filtered); super.set(current); this.current.set(current); From 454d264555a0f934a213597899b58d56dcd33afc Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Tue, 2 Apr 2024 12:09:09 -0500 Subject: [PATCH 33/47] added a comment --- .../robotics/math/filters/AlphaFilteredRigidBodyTransform.java | 1 + 1 file changed, 1 insertion(+) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java index ded68f13..159f413f 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java @@ -4,6 +4,7 @@ public class AlphaFilteredRigidBodyTransform extends RigidBodyTransform { + // an alpha of zero applies zero filtering, accepting the entirely new input. private double alpha = 0.0; private final RigidBodyTransform previousFiltered = new RigidBodyTransform(); From 639d939d7e26636c30f2ff2204a9158cdf635ed5 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Thu, 23 May 2024 10:06:31 -0500 Subject: [PATCH 34/47] Reduced constructor requirement. --- .../math/filters/DelayedYoDouble.java | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java index 391f2239..55da3af5 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java @@ -1,15 +1,16 @@ package us.ihmc.robotics.math.filters; +import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; public class DelayedYoDouble extends YoDouble { - private final YoDouble variableToDelay; + private final DoubleProvider variableToDelay; private final YoDouble[] previousYoVariables; - public DelayedYoDouble(String name, String description, YoDouble variableToDelay, int ticksToDelay, YoRegistry registry) + public DelayedYoDouble(String name, String description, DoubleProvider variableToDelay, int ticksToDelay, YoRegistry registry) { super(name, description, registry); @@ -19,36 +20,36 @@ public DelayedYoDouble(String name, String description, YoDouble variableToDelay for (int i = 0; i < ticksToDelay; i++) { previousYoVariables[i] = new YoDouble(name + "_previous" + i, registry); - previousYoVariables[i].set(variableToDelay.getDoubleValue()); + previousYoVariables[i].set(variableToDelay.getValue()); } - this.set(variableToDelay.getDoubleValue()); + this.set(variableToDelay.getValue()); } public void update() { if (previousYoVariables.length == 0) { - this.set(variableToDelay.getDoubleValue()); + this.set(variableToDelay.getValue()); return; } - this.set(previousYoVariables[0].getDoubleValue()); + this.set(previousYoVariables[0].getValue()); for (int i = 0; i < previousYoVariables.length - 1; i++) { - previousYoVariables[i].set(previousYoVariables[i + 1].getDoubleValue()); + previousYoVariables[i].set(previousYoVariables[i + 1].getValue()); } - previousYoVariables[previousYoVariables.length - 1].set(variableToDelay.getDoubleValue()); + previousYoVariables[previousYoVariables.length - 1].set(variableToDelay.getValue()); } public void reset() { for (YoDouble var : previousYoVariables) { - var.set(variableToDelay.getDoubleValue()); + var.set(variableToDelay.getValue()); } - this.set(variableToDelay.getDoubleValue()); + this.set(variableToDelay.getValue()); } } From f799dfe9d149a5a1907553cf512d76681658a664 Mon Sep 17 00:00:00 2001 From: Tomasz Bialek Date: Mon, 20 May 2024 15:10:21 -0500 Subject: [PATCH 35/47] Added alpha filtered position --- .../math/filters/AlphaFilteredTuple3D.java | 163 ++++++++++++++++++ 1 file changed, 163 insertions(+) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple3D.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple3D.java new file mode 100644 index 00000000..bfb24da9 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple3D.java @@ -0,0 +1,163 @@ +package us.ihmc.robotics.math.filters; + +import org.apache.commons.lang3.NotImplementedException; +import us.ihmc.euclid.interfaces.EuclidGeometry; +import us.ihmc.euclid.interfaces.Settable; +import us.ihmc.euclid.tools.EuclidCoreIOTools; +import us.ihmc.euclid.tools.EuclidCoreTools; +import us.ihmc.euclid.transform.interfaces.Transform; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; + +public class AlphaFilteredTuple3D implements Tuple3DBasics, Settable +{ + private double alpha; + + private double x = Double.NaN; + private double y = Double.NaN; + private double z = Double.NaN; + + private boolean resetX = false; + private boolean resetY = false; + private boolean resetZ = false; + + public AlphaFilteredTuple3D() + { + this.alpha = 0.0; + } + + public AlphaFilteredTuple3D(double alpha) + { + this.alpha = alpha; + } + + public AlphaFilteredTuple3D(double x, double y, double z, double alpha) + { + this.alpha = alpha; + beginReset(); + set(x, y, z); + } + + public AlphaFilteredTuple3D(Tuple3DReadOnly other, double alpha) + { + this.alpha = alpha; + beginReset(); + set(other); + } + + public void beginReset() + { + resetX = true; + resetY = true; + resetZ = true; + } + + public void endReset() + { + resetX = false; + resetY = false; + resetZ = false; + } + + public void reset(Tuple3DReadOnly other) + { + beginReset(); + set(other); + } + + @Override + public void setX(double x) + { + if (resetX || Double.isNaN(this.x)) + { + this.x = x; + resetX = false; + } + else + { + this.x = EuclidCoreTools.interpolate(x, this.x, alpha); + } + } + + @Override + public void setY(double y) + { + if (resetY || Double.isNaN(this.y)) + { + this.y = y; + resetY = false; + } + else + { + this.y = EuclidCoreTools.interpolate(y, this.y, alpha); + } + } + + @Override + public void setZ(double z) + { + if (resetZ || Double.isNaN(this.z)) + { + this.z = z; + resetZ = false; + } + else + { + this.z = EuclidCoreTools.interpolate(z, this.z, alpha); + } + } + + public void setAlpha(double alpha) + { + this.alpha = alpha; + } + + @Override + public void applyTransform(Transform transform) + { + throw new NotImplementedException("Sorry mate, " + getClass().getSimpleName() + " doesn't implement this method."); + } + + @Override + public void applyInverseTransform(Transform transform) + { + throw new NotImplementedException("Sorry mate, " + getClass().getSimpleName() + " doesn't implement this method."); + } + + @Override + public double getX() + { + return x; + } + + @Override + public double getY() + { + return y; + } + + @Override + public double getZ() + { + return z; + } + + @Override + public boolean geometricallyEquals(EuclidGeometry geometry, double epsilon) + { + return epsilonEquals(geometry, epsilon); + } + + @Override + public void set(Tuple3DBasics other) + { + beginReset(); + interpolate(other, this, alpha); + } + + @Override + public String toString() + { + return toString(EuclidCoreIOTools.DEFAULT_FORMAT); + } +} From 9d83d0fa110e92a07909521876005d42309b520f Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Tue, 6 Aug 2024 07:47:28 -0500 Subject: [PATCH 36/47] deleted minimum jerk trajectory and deprecated trapesoidal velocity trajectory and its implementations. Hexapod still depends on it. delete matrix yo variable converter tools delete some old yo variable tools delete some unused yo variable tools delete unused stiction model tools delete unused alpha to alpha functions deleted some controller classes, and left others to be reviewed deprecated some straght line tests, they need to be pushed into the striaght line pose traejctory removed straight line poisition trajectory from the pose test removed the straight line independent trajectory generators that werent used deleted the unused qp code deleted projection tools started switching away from random geometry class and the old robotics assert converted more of the tests to the right format switched getting all the current crop of moving away from random geometry working got rid of the custom random matrix generator class deleted the random geometry class, since its duplicated by other tools fixed some deletes and removed some provider structure that did nothing fixed some more deletes moved the planar landmark to the correct place deleted many other classes delete unused code deleted a whole bunch of stuff from the java toolkit deleted the simple walking controller --- .../src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java index 1ebf58e5..d6371f75 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java @@ -72,6 +72,7 @@ public YoMatrix(String name, String description, int maxNumberOfRows, int maxNum case NONE: { variables[row][column] = new YoDouble(name + row + column, description, registry); // names are simply the row and column indices + variables[row][column].setToNaN(); break; } case ROWS: @@ -81,11 +82,13 @@ public YoMatrix(String name, String description, int maxNumberOfRows, int maxNum "The YoMatrix must be a column vector if only row names are provided, else unique names cannot be generated."); variables[row][column] = new YoDouble(name + rowNames[row], description, registry); // names are the row names, no column identifier + variables[row][column].setToNaN(); break; } case ROWS_AND_COLUMNS: { variables[row][column] = new YoDouble(name + rowNames[row] + columnNames[column], description, registry); // names are the row and column names + variables[row][column].setToNaN(); break; } } From 3d5b81c7ca5028f5cb4d5dbf6fa06ae15a1ce30b Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Tue, 24 Sep 2024 22:42:47 -0500 Subject: [PATCH 37/47] made yo matrix more efficient added a to string for the yo matrix fixed some tests for yo matrix. They failed based on the way that the yo matrix gets initialized, and a change in the implementation of the zero method --- .../ihmc/robotics/math/frames/YoMatrix.java | 52 +++++++++++++++---- 1 file changed, 41 insertions(+), 11 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java index d6371f75..df502b39 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java @@ -6,6 +6,9 @@ import us.ihmc.yoVariables.variable.YoDouble; import us.ihmc.yoVariables.variable.YoInteger; +import java.io.ByteArrayOutputStream; +import java.io.PrintStream; + /** * YoMatrix. Object for holding a matrix of YoVariables so that Matrices can be rewound. Has a * maximum number of rows and columns and an actual number of rows and columns. If you set with a @@ -71,7 +74,7 @@ public YoMatrix(String name, String description, int maxNumberOfRows, int maxNum { case NONE: { - variables[row][column] = new YoDouble(name + row + column, description, registry); // names are simply the row and column indices + variables[row][column] = new YoDouble(getFieldName(name, row, column), description, registry); // names are simply the row and column indices variables[row][column].setToNaN(); break; } @@ -81,13 +84,13 @@ public YoMatrix(String name, String description, int maxNumberOfRows, int maxNum throw new IllegalArgumentException( "The YoMatrix must be a column vector if only row names are provided, else unique names cannot be generated."); - variables[row][column] = new YoDouble(name + rowNames[row], description, registry); // names are the row names, no column identifier + variables[row][column] = new YoDouble(getFieldName(name, rowNames[row], ""), description, registry); // names are the row names, no column identifier variables[row][column].setToNaN(); break; } case ROWS_AND_COLUMNS: { - variables[row][column] = new YoDouble(name + rowNames[row] + columnNames[column], description, registry); // names are the row and column names + variables[row][column] = new YoDouble(getFieldName(name, rowNames[row], columnNames[column]), description, registry); // names are the row and column names variables[row][column].setToNaN(); break; } @@ -96,6 +99,16 @@ public YoMatrix(String name, String description, int maxNumberOfRows, int maxNum } } + public static String getFieldName(String prefix, int row, int column) + { + return getFieldName(prefix, "_" + row, "_" + column); + } + + public static String getFieldName(String prefix, String rowName, String columName) + { + return prefix + rowName + columName; + } + private enum NamesProvided { NONE, ROWS, ROWS_AND_COLUMNS @@ -136,7 +149,12 @@ public void set(int row, int col, double val) @Override public void unsafe_set(int row, int col, double val) { - variables[row][col].set(val); + unsafe_set(row, col, val, true); + } + + private void unsafe_set(int row, int col, double val, boolean notifyListeners) + { + variables[row][col].set(val, notifyListeners); } @Override @@ -197,7 +215,7 @@ public void set(Matrix original) { for (int col = 0; col < getNumCols(); col++) { - set(row, col, otherMatrix.unsafe_get(row, col)); + unsafe_set(row, col, otherMatrix.unsafe_get(row, col), false); } } } @@ -238,7 +256,7 @@ else if (numRows < 0 || numCols < 0) { for (int col = numCols; col < maxNumberOfColumns; col++) { - unsafe_set(row, col, Double.NaN); + unsafe_set(row, col, Double.NaN, false); } } @@ -246,7 +264,7 @@ else if (numRows < 0 || numCols < 0) { for (int col = 0; col < maxNumberOfColumns; col++) { - unsafe_set(row, col, Double.NaN); + unsafe_set(row, col, Double.NaN, false); } } } @@ -266,14 +284,17 @@ public void set(DMatrix matrix) { for (int column = 0; column < maxNumberOfColumns; column++) { + double value; if ((row < numRows) && (column < numCols)) { - variables[row][column].set(matrix.get(row, column)); + value = matrix.unsafe_get(row, column); } else { - variables[row][column].set(Double.NaN); + value = Double.NaN; } + unsafe_set(row, column, value, false); + } } } @@ -298,7 +319,7 @@ public void get(DMatrix matrixToPack) { for (int column = 0; column < numCols; column++) { - matrixToPack.set(row, column, variables[row][column].getDoubleValue()); + matrixToPack.unsafe_set(row, column, variables[row][column].getDoubleValue()); } } } @@ -310,7 +331,7 @@ public void setToNaN(int numberOfRows, int numberOfColumns) { for (int col = 0; col < numberOfColumns; col++) { - unsafe_set(row, col, Double.NaN); + unsafe_set(row, col, Double.NaN, false); } } } @@ -319,4 +340,13 @@ public YoDouble getYoDouble(int row, int col) { return variables[row][col]; } + + @Override + public String toString() + { + ByteArrayOutputStream stream = new ByteArrayOutputStream(); + MatrixIO.print(new PrintStream(stream), this); + + return stream.toString(); + } } From 3f4624e5e532a74b34532eeb13d7f673d2059474 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Sun, 13 Oct 2024 22:50:52 -0500 Subject: [PATCH 38/47] started moving a bunch of packages to allow extracting the whole-body control core significantly overhauled and set up custom pid parameters for the whole body controller core moved a lot of the filtered yo variables into a common project that can be moved to commons moved a bunch of the polynomial classes out after getting things solidified, but need to fix the linear systems tests cleaned up a lot more of the yo variable types and moved their packages got more code moved and most of the linear algebra tests fixed removed unused code set up wbcc for extraction --- .../AccelerationLimitedYoFrameVector2d.java | 85 ----- .../AccelerationLimitedYoFrameVector3D.java | 173 --------- .../AccelerationLimitedYoVariable.java | 138 ------- .../AlphaBasedOnBreakFrequencyProvider.java | 84 ----- .../AlphaFilteredRigidBodyTransform.java | 43 --- .../math/filters/AlphaFilteredTuple2D.java | 103 ----- .../math/filters/AlphaFilteredTuple3D.java | 163 -------- .../AlphaFilteredWrappingYoVariable.java | 139 ------- .../filters/AlphaFilteredYoFramePoint.java | 144 ------- .../filters/AlphaFilteredYoFramePoint2d.java | 156 -------- .../filters/AlphaFilteredYoFramePose3D.java | 128 ------- .../AlphaFilteredYoFrameQuaternion.java | 113 ------ .../filters/AlphaFilteredYoFrameVector.java | 146 -------- .../filters/AlphaFilteredYoFrameVector2d.java | 170 --------- .../math/filters/AlphaFilteredYoMatrix.java | 109 ------ .../math/filters/AlphaFilteredYoVariable.java | 181 --------- .../math/filters/AverageYoDouble.java | 52 --- .../math/filters/AverageYoFrameVector3D.java | 78 ---- ...lashCompensatingVelocityYoFrameVector.java | 77 ---- ...acklashCompensatingVelocityYoVariable.java | 242 ------------ .../filters/BacklashProcessingYoVariable.java | 153 -------- .../filters/BetaFilteredYoFramePoint2d.java | 85 ----- .../filters/BetaFilteredYoFrameVector2d.java | 75 ---- .../math/filters/BetaFilteredYoVariable.java | 105 ------ .../math/filters/DeadzoneYoFrameVector.java | 72 ---- .../math/filters/DeadzoneYoVariable.java | 69 ---- .../math/filters/DelayedYoBoolean.java | 68 ---- .../math/filters/DelayedYoDouble.java | 55 --- .../math/filters/DeltaLimitedYoVariable.java | 69 ---- .../FilteredDiscreteVelocityYoVariable.java | 201 ---------- .../FilteredVelocityYoFrameVector.java | 134 ------- .../FilteredVelocityYoFrameVector2d.java | 126 ------- .../filters/FilteredVelocityYoVariable.java | 182 --------- ...ifferenceAngularVelocityYoFrameVector.java | 97 ----- .../FirstOrderBandPassFilteredYoVariable.java | 68 ---- .../filters/FirstOrderFilteredYoVariable.java | 157 -------- .../math/filters/GlitchFilteredYoBoolean.java | 122 ------ .../math/filters/GlitchFilteredYoInteger.java | 107 ------ .../math/filters/JerkLimitedYoVariable.java | 151 -------- .../math/filters/ProcessingYoVariable.java | 10 - .../RateLimitedYoFrameOrientation.java | 152 -------- .../math/filters/RateLimitedYoFramePoint.java | 120 ------ .../filters/RateLimitedYoFramePoint2D.java | 134 ------- .../math/filters/RateLimitedYoFramePose.java | 110 ------ .../filters/RateLimitedYoFrameQuaternion.java | 151 -------- .../filters/RateLimitedYoFrameVector.java | 120 ------ .../filters/RateLimitedYoFrameVector2d.java | 102 ----- .../RateLimitedYoMutableFrameVector3D.java | 121 ------ .../math/filters/RateLimitedYoVariable.java | 118 ------ ...acklashCompensatingVelocityYoVariable.java | 206 ---------- .../SecondOrderFilteredYoFrameVector.java | 97 ----- ...condOrderFilteredYoVariableParameters.java | 36 -- ...pleMovingAverageFilteredYoFrameVector.java | 94 ----- ...SimpleMovingAverageFilteredYoVariable.java | 87 ----- .../ihmc/robotics/math/frames/YoMatrix.java | 352 ------------------ 55 files changed, 6630 deletions(-) delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple3D.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoDouble.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoFrameVector3D.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java deleted file mode 100644 index 5bbdf29d..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector2d.java +++ /dev/null @@ -1,85 +0,0 @@ -package us.ihmc.robotics.math.filters; - - - -import us.ihmc.euclid.referenceFrame.FrameVector2D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.tuple2D.Vector2D; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.tools.YoGeometryNameTools; - -public class AccelerationLimitedYoFrameVector2d extends YoFrameVector2D -{ - private final AccelerationLimitedYoVariable x, y; - - private AccelerationLimitedYoFrameVector2d(AccelerationLimitedYoVariable x, AccelerationLimitedYoVariable y, ReferenceFrame referenceFrame) - { - super(x, y, referenceFrame); - - this.x = x; - this.y = y; - } - - public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, - DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt, ReferenceFrame referenceFrame) - { - AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); - AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, dt); - - AccelerationLimitedYoFrameVector2d ret = new AccelerationLimitedYoFrameVector2d(x, y, referenceFrame); - - return ret; - } - - - public static AccelerationLimitedYoFrameVector2d createAccelerationLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, - DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt, YoFrameVector2D unfilteredVector) - { - AccelerationLimitedYoVariable x = new AccelerationLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoX(), dt); - AccelerationLimitedYoVariable y = new AccelerationLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, maxAcceleration, unfilteredVector.getYoY(), dt); - - AccelerationLimitedYoFrameVector2d ret = new AccelerationLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); - - return ret; - } - - public void setGainsByPolePlacement(double w0, double zeta) - { - x.setGainsByPolePlacement(w0, zeta); - y.setGainsByPolePlacement(w0, zeta); - } - - public void update() - { - x.update(); - y.update(); - } - - public void update(double xUnfiltered, double yUnfiltered) - { - x.update(xUnfiltered); - y.update(yUnfiltered); - } - - public void update(Vector2D vector2dUnfiltered) - { - x.update(vector2dUnfiltered.getX()); - y.update(vector2dUnfiltered.getY()); - } - - public void update(FrameVector2D vector2dUnfiltered) - { - x.update(vector2dUnfiltered.getX()); - y.update(vector2dUnfiltered.getY()); - } - - public void reset() - { - x.reset(); - y.reset(); - } -} - - diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java deleted file mode 100644 index 2873da53..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoFrameVector3D.java +++ /dev/null @@ -1,173 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.FrameVector3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class AccelerationLimitedYoFrameVector3D extends YoFrameVector3D -{ - private final DoubleProvider maxRateVariable; - private final DoubleProvider maxAccelerationVariable; - - private final FrameTuple3DReadOnly rawPosition; - private final YoBoolean accelerationLimited; - private final YoBoolean rateLimited; - - private final YoBoolean hasBeenInitialized; - private final YoDouble positionGain; - private final YoDouble velocityGain; - - private final YoFrameVector3D smoothedRate; - private final YoFrameVector3D smoothedAcceleration; - - private final double dt; - - private final FrameVector3D positionError; - private final FrameVector3D acceleration; - - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); - maxRate.set(initialValue); - return maxRate; - } - - private static DoubleProvider createMaxAccelerationYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "MaxAcceleration" + nameSuffix, registry); - maxRate.set(initialValue); - return maxRate; - } - - public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, - DoubleProvider maxAcceleration, double dt, FrameTuple3DReadOnly rawPosition) - { - this(namePrefix, nameSuffix, registry, maxRate, maxAcceleration, dt, rawPosition, rawPosition.getReferenceFrame()); - } - - public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, - DoubleProvider maxAcceleration, double dt, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, maxRate, maxAcceleration, dt, null, referenceFrame); - } - public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double maxAcceleration, - double dt, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), - createMaxAccelerationYoDouble(namePrefix, nameSuffix, maxAcceleration, registry), dt, null, referenceFrame); - } - - private AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, - DoubleProvider maxAcceleration, double dt, FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - positionError = new FrameVector3D(referenceFrame); - acceleration = new FrameVector3D(referenceFrame); - - if (maxRate == null) - maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); - if (maxAcceleration == null) - maxAcceleration = createMaxAccelerationYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); - - this.maxRateVariable = maxRate; - this.maxAccelerationVariable = maxAcceleration; - - this.dt = dt; - - hasBeenInitialized = new YoBoolean(namePrefix + "HasBeenInitialized" + namePrefix, registry); - this.rateLimited = new YoBoolean(namePrefix + "RateLimited" + nameSuffix, registry); - this.accelerationLimited = new YoBoolean(namePrefix + "AccelerationLimited" + nameSuffix, registry); - - smoothedRate = new YoFrameVector3D(namePrefix + "SmoothedRate" + namePrefix, referenceFrame, registry); - smoothedAcceleration = new YoFrameVector3D(namePrefix + "SmoothedAcceleration" + namePrefix, referenceFrame, registry); - - positionGain = new YoDouble(namePrefix + "PositionGain" + namePrefix, registry); - velocityGain = new YoDouble(namePrefix + "VelocityGain" + namePrefix, registry); - - double w0 = 2.0 * Math.PI * 16.0; - double zeta = 1.0; - - setGainsByPolePlacement(w0, zeta); - hasBeenInitialized.set(false); - - this.rawPosition = rawPosition; - - } - - - public void setGainsByPolePlacement(double w0, double zeta) - { - positionGain.set(w0 * w0); - velocityGain.set(2.0 * zeta * w0); - } - - public void update() - { - if (rawPosition == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(rawPosition); - } - - public void update(FrameTuple3DReadOnly frameVectorUnfiltered) - { - checkReferenceFrameMatch(frameVectorUnfiltered); - update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); - } - - public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) - { - if (!hasBeenInitialized.getBooleanValue()) - initialize(xUnfiltered, yUnfiltered, zUnfiltered); - - positionError.set(xUnfiltered, yUnfiltered, zUnfiltered); - positionError.sub(this); - - acceleration.set(smoothedRate); - acceleration.scale(-velocityGain.getValue()); - acceleration.scaleAdd(positionGain.getValue(), positionError, acceleration); - - accelerationLimited.set(acceleration.clipToMaxLength(maxAccelerationVariable.getValue())); - - smoothedAcceleration.set(acceleration); - smoothedRate.scaleAdd(dt, smoothedAcceleration, smoothedRate); - - rateLimited.set(smoothedRate.clipToMaxLength(maxRateVariable.getValue())); - - this.scaleAdd(dt, smoothedRate, this); - - if (this.containsNaN()) - throw new RuntimeException("what?"); - - } - - public void initialize(FrameTuple3DReadOnly input) - { - initialize(input.getX(), input.getY(), input.getZ()); - } - - public void initialize(double xInput, double yInput, double zInput) - { - this.set(xInput, yInput, zInput); - smoothedRate.setToZero(); - smoothedAcceleration.setToZero(); - - this.hasBeenInitialized.set(true); - } - - public void reset() - { - this.hasBeenInitialized.set(false); - smoothedRate.setToZero(); - smoothedAcceleration.setToZero(); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java deleted file mode 100644 index 2d3adb8c..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AccelerationLimitedYoVariable.java +++ /dev/null @@ -1,138 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.commons.MathTools; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class AccelerationLimitedYoVariable extends YoDouble -{ - private final double dt; - - private final YoBoolean hasBeenInitialized; - - private final YoDouble smoothedRate; - private final YoDouble smoothedAcceleration; - - private final YoDouble positionGain; - private final YoDouble velocityGain; - - private DoubleProvider maximumRate; - private DoubleProvider maximumAcceleration; - - private final DoubleProvider inputVariable; - - public AccelerationLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt) - { - this(name, registry, maxRate, maxAcceleration, null, dt); - } - - public AccelerationLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, - DoubleProvider inputVariable, double dt) - { - super(name, registry); - - if (maxRate != null && maxAcceleration != null) - { - this.maximumRate = maxRate; - this.maximumAcceleration = maxAcceleration; - } - - this.dt = dt; - - hasBeenInitialized = new YoBoolean(name + "HasBeenInitialized", registry); - - smoothedRate = new YoDouble(name + "SmoothedRate", registry); - smoothedAcceleration = new YoDouble(name + "SmoothedAcceleration", registry); - - positionGain = new YoDouble(name + "PositionGain", registry); - velocityGain = new YoDouble(name + "VelocityGain", registry); - - double w0 = 2.0 * Math.PI * 16.0; - double zeta = 1.0; - - setGainsByPolePlacement(w0, zeta); - hasBeenInitialized.set(false); - - this.inputVariable = inputVariable; - } - - - public void setGainsByPolePlacement(double w0, double zeta) - { - positionGain.set(w0 * w0); - velocityGain.set(2.0 * zeta * w0); - } - - public YoDouble getPositionGain() - { - return positionGain; - } - - public YoDouble getVelocityGain() - { - return velocityGain; - } - - public void update() - { - update(inputVariable.getValue()); - } - - public void update(double input) - { - if (!hasBeenInitialized.getBooleanValue()) - initialize(input); - - double positionError = input - this.getDoubleValue(); - double acceleration = -velocityGain.getDoubleValue() * smoothedRate.getDoubleValue() + positionGain.getDoubleValue() * positionError; - acceleration = MathTools.clamp(acceleration, -maximumAcceleration.getValue(), maximumAcceleration.getValue()); - - smoothedAcceleration.set(acceleration); - smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); - smoothedRate.set(MathTools.clamp(smoothedRate.getDoubleValue(), maximumRate.getValue())); - this.add(smoothedRate.getDoubleValue() * dt); - } - - public void initialize(double input) - { - this.set(input); - smoothedRate.set(0.0); - smoothedAcceleration.set(0.0); - - this.hasBeenInitialized.set(true); - } - - public void reset() - { - this.hasBeenInitialized.set(false); - smoothedRate.set(0.0); - smoothedAcceleration.set(0.0); - } - - public YoDouble getSmoothedRate() - { - return smoothedRate; - } - - public YoDouble getSmoothedAcceleration() - { - return smoothedAcceleration; - } - - public boolean hasBeenInitialized() - { - return hasBeenInitialized.getBooleanValue(); - } - - public double getMaximumRate() - { - return maximumRate.getValue(); - } - - public double getMaximumAcceleration() - { - return maximumAcceleration.getValue(); - } -} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java deleted file mode 100644 index d9235435..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBasedOnBreakFrequencyProvider.java +++ /dev/null @@ -1,84 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.parameters.DoubleParameter; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; - -/** - * This class calculates the alpha value given a break frequency provider - * - * The value gets cached to avoid unnecessary calculations - * - * @author Jesper Smith - * - */ -public class AlphaBasedOnBreakFrequencyProvider implements DoubleProvider -{ - - private final DoubleProvider breakFrequencyProvider; - private final double dt; - - private double previousBreakFrequency = Double.NaN; - private double alpha = 0.0; - - /** - * Create a new provider using the break frequency provided by double provider - * - * @param breakFrequencyProvider - * @param dt - */ - public AlphaBasedOnBreakFrequencyProvider(DoubleProvider breakFrequencyProvider, double dt) - { - this.breakFrequencyProvider = breakFrequencyProvider; - this.dt = dt; - } - - - /** - * Create a new provider backed by a frequency with the given name. - * - * @param name Name of the break frequency parameter - * @param dt Time step - * @param registry Parent registry for the break frequency parameter - * @param defaultBreakFrequency Default value for the break frequency - */ - public AlphaBasedOnBreakFrequencyProvider(String name, double dt, YoRegistry registry, double defaultBreakFrequency) - { - this(new DoubleParameter(name, registry, defaultBreakFrequency), dt); - } - - /** - * Create a new provider backed by a frequency with the given name. - * - * @param name Name of the break frequency parameter - * @param dt Time step - * @param registry Parent registry for the break frequency parameter - */ - public AlphaBasedOnBreakFrequencyProvider(String name, double dt, YoRegistry registry) - { - this.breakFrequencyProvider = new DoubleParameter(name, registry, Double.NaN); - this.dt = dt; - } - - /** - * 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; - } - -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java deleted file mode 100644 index 159f413f..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredRigidBodyTransform.java +++ /dev/null @@ -1,43 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.transform.RigidBodyTransform; - -public class AlphaFilteredRigidBodyTransform extends RigidBodyTransform -{ - // an alpha of zero applies zero filtering, accepting the entirely new input. - private double alpha = 0.0; - private final RigidBodyTransform previousFiltered = new RigidBodyTransform(); - - public AlphaFilteredRigidBodyTransform() - { - reset(); - } - - public void update(RigidBodyTransform measured) - { - if (containsNaN()) - { - set(measured); - } - else - { - previousFiltered.set(this); - interpolate(measured, previousFiltered, alpha); - } - } - - public double getAlpha() - { - return alpha; - } - - public void setAlpha(double alpha) - { - this.alpha = alpha; - } - - public void reset() - { - setToNaN(); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java deleted file mode 100644 index f9523e64..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple2D.java +++ /dev/null @@ -1,103 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import org.apache.commons.lang3.NotImplementedException; - -import us.ihmc.euclid.interfaces.EuclidGeometry; -import us.ihmc.euclid.transform.interfaces.Transform; -import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; -import us.ihmc.yoVariables.providers.DoubleProvider; - -public class AlphaFilteredTuple2D implements Tuple2DBasics -{ - private final DoubleProvider alpha; - - private boolean resetX; - private boolean resetY; - - private double x; - private double y; - - public AlphaFilteredTuple2D(DoubleProvider alpha) - { - this.alpha = alpha; - reset(); - } - - public AlphaFilteredTuple2D(Tuple2DReadOnly other, DoubleProvider alpha) - { - this.alpha = alpha; - reset(other); - } - - public void reset() - { - resetX = true; - resetY = true; - } - - public void reset(Tuple2DReadOnly other) - { - resetX = true; - resetY = true; - set(other); - } - - @Override - public double getX() - { - return x; - } - - @Override - public double getY() - { - return y; - } - - @Override - public void setX(double x) - { - if (resetX) - { - this.x = x; - resetX = false; - } - else - { - this.x = alpha.getValue() * this.x + (1.0 - alpha.getValue()) * x; - } - } - - @Override - public void setY(double y) - { - if (resetY) - { - this.y = y; - resetY = false; - } - else - { - this.y = alpha.getValue() * this.y + (1.0 - alpha.getValue()) * y; - } - } - - @Override - public void applyTransform(Transform transform, boolean checkIfTransformInXYPlane) - { - throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + "."); - } - - @Override - public void applyInverseTransform(Transform transform, boolean checkIfTransformInXYPlane) - { - throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + "."); - } - - @Override - public boolean geometricallyEquals(EuclidGeometry geometry, double epsilon) - { - return epsilonEquals(geometry, epsilon); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple3D.java deleted file mode 100644 index bfb24da9..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredTuple3D.java +++ /dev/null @@ -1,163 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import org.apache.commons.lang3.NotImplementedException; -import us.ihmc.euclid.interfaces.EuclidGeometry; -import us.ihmc.euclid.interfaces.Settable; -import us.ihmc.euclid.tools.EuclidCoreIOTools; -import us.ihmc.euclid.tools.EuclidCoreTools; -import us.ihmc.euclid.transform.interfaces.Transform; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; - -public class AlphaFilteredTuple3D implements Tuple3DBasics, Settable -{ - private double alpha; - - private double x = Double.NaN; - private double y = Double.NaN; - private double z = Double.NaN; - - private boolean resetX = false; - private boolean resetY = false; - private boolean resetZ = false; - - public AlphaFilteredTuple3D() - { - this.alpha = 0.0; - } - - public AlphaFilteredTuple3D(double alpha) - { - this.alpha = alpha; - } - - public AlphaFilteredTuple3D(double x, double y, double z, double alpha) - { - this.alpha = alpha; - beginReset(); - set(x, y, z); - } - - public AlphaFilteredTuple3D(Tuple3DReadOnly other, double alpha) - { - this.alpha = alpha; - beginReset(); - set(other); - } - - public void beginReset() - { - resetX = true; - resetY = true; - resetZ = true; - } - - public void endReset() - { - resetX = false; - resetY = false; - resetZ = false; - } - - public void reset(Tuple3DReadOnly other) - { - beginReset(); - set(other); - } - - @Override - public void setX(double x) - { - if (resetX || Double.isNaN(this.x)) - { - this.x = x; - resetX = false; - } - else - { - this.x = EuclidCoreTools.interpolate(x, this.x, alpha); - } - } - - @Override - public void setY(double y) - { - if (resetY || Double.isNaN(this.y)) - { - this.y = y; - resetY = false; - } - else - { - this.y = EuclidCoreTools.interpolate(y, this.y, alpha); - } - } - - @Override - public void setZ(double z) - { - if (resetZ || Double.isNaN(this.z)) - { - this.z = z; - resetZ = false; - } - else - { - this.z = EuclidCoreTools.interpolate(z, this.z, alpha); - } - } - - public void setAlpha(double alpha) - { - this.alpha = alpha; - } - - @Override - public void applyTransform(Transform transform) - { - throw new NotImplementedException("Sorry mate, " + getClass().getSimpleName() + " doesn't implement this method."); - } - - @Override - public void applyInverseTransform(Transform transform) - { - throw new NotImplementedException("Sorry mate, " + getClass().getSimpleName() + " doesn't implement this method."); - } - - @Override - public double getX() - { - return x; - } - - @Override - public double getY() - { - return y; - } - - @Override - public double getZ() - { - return z; - } - - @Override - public boolean geometricallyEquals(EuclidGeometry geometry, double epsilon) - { - return epsilonEquals(geometry, epsilon); - } - - @Override - public void set(Tuple3DBasics other) - { - beginReset(); - interpolate(other, this, alpha); - } - - @Override - public String toString() - { - return toString(EuclidCoreIOTools.DEFAULT_FORMAT); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java deleted file mode 100644 index b204d605..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredWrappingYoVariable.java +++ /dev/null @@ -1,139 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.commons.MathTools; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; - -public class AlphaFilteredWrappingYoVariable extends AlphaFilteredYoVariable -{ - public static final double EPSILON = 1e-10; - - private double previousUnfilteredVariable; - private final YoDouble unfilteredVariable; - private final YoDouble unfilteredInRangeVariable; - private final DoubleProvider alphaVariable; - - private final YoDouble temporaryOutputVariable; - private final YoDouble error; - private final double upperLimit; - private final double lowerLimit; - private final double range; - - //wrap the values in [lowerLimit ; upperLimit[ - - public AlphaFilteredWrappingYoVariable(String name, String description, YoRegistry registry, final YoDouble unfilteredVariable, DoubleProvider alphaVariable, double lowerLimit, double upperLimit) - { - super(name, description, registry, alphaVariable); - this.alphaVariable = alphaVariable; - this.upperLimit = upperLimit; - this.lowerLimit = lowerLimit; - this.range = upperLimit - lowerLimit; - this.unfilteredVariable = unfilteredVariable; - - unfilteredInRangeVariable = new YoDouble(name + "UnfilteredInRangeVariable", registry); - temporaryOutputVariable = new YoDouble(name + "TemporaryOutputVariable", registry); - error = new YoDouble(name + "Error", registry); - - } - - @Override - public void update() - { - update(unfilteredVariable.getDoubleValue()); - } - - - @Override - public void update(double currentPosition) - { - if(!hasBeenCalled.getBooleanValue()) - { - hasBeenCalled.set(true); - previousUnfilteredVariable = unfilteredVariable.getDoubleValue(); - - unfilteredVariableModulo(currentPosition); - - temporaryOutputVariable.set(unfilteredInRangeVariable.getDoubleValue()); - set(unfilteredInRangeVariable.getDoubleValue()); - } - else - { - if (!MathTools.epsilonEquals(currentPosition, previousUnfilteredVariable, EPSILON)) - { - previousUnfilteredVariable = currentPosition; - - unfilteredVariableModulo(currentPosition); - - //calculate the error - double standardError = unfilteredInRangeVariable.getDoubleValue() - getDoubleValue(); - double wrappingError; - if(unfilteredInRangeVariable.getDoubleValue() > getDoubleValue()) - { - wrappingError = lowerLimit - getDoubleValue() + unfilteredInRangeVariable.getDoubleValue() - upperLimit; - } - else - { - wrappingError = upperLimit - getDoubleValue() + unfilteredInRangeVariable.getDoubleValue() - lowerLimit; - } - if(Math.abs(standardError) < Math.abs(wrappingError)) - { - error.set(standardError); - } - else - { - error.set(wrappingError); - } - - //determine if wrapping and move the input if necessary - temporaryOutputVariable.set(getDoubleValue()); - if ((getDoubleValue() + error.getDoubleValue()) >= upperLimit) - { - temporaryOutputVariable.set(getDoubleValue() - range); - } - if ((getDoubleValue() + error.getDoubleValue()) < lowerLimit) - { - temporaryOutputVariable.set(getDoubleValue() + range); - } - } - - temporaryOutputVariable.set(alphaVariable.getValue() * temporaryOutputVariable.getDoubleValue() + (1.0 - alphaVariable.getValue()) - * unfilteredInRangeVariable.getDoubleValue()); - - if (temporaryOutputVariable.getDoubleValue() > upperLimit + EPSILON) - { - set(temporaryOutputVariable.getDoubleValue() - range); - } - else if (temporaryOutputVariable.getDoubleValue() <= lowerLimit - EPSILON) - { - set(temporaryOutputVariable.getDoubleValue() + range); - } - else - { - set(temporaryOutputVariable.getDoubleValue()); - } - } - } - - private void unfilteredVariableModulo(double currentPosition) - { - //handle if the input is out of range - boolean rangeNeedsToBeChecked = true; - unfilteredInRangeVariable.set(currentPosition); - - while(rangeNeedsToBeChecked) - { - rangeNeedsToBeChecked = false; - if (unfilteredInRangeVariable.getDoubleValue() >= upperLimit) - { - unfilteredInRangeVariable.set(unfilteredInRangeVariable.getDoubleValue() - range); - rangeNeedsToBeChecked = true; - } - if (unfilteredInRangeVariable.getDoubleValue() < lowerLimit) - { - unfilteredInRangeVariable.set(unfilteredInRangeVariable.getDoubleValue() + range); - rangeNeedsToBeChecked = true; - } - } - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java deleted file mode 100644 index b4aaf0c3..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint.java +++ /dev/null @@ -1,144 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.euclid.tuple3D.Point3D; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; - -public class AlphaFilteredYoFramePoint extends YoFramePoint3D -{ - private final DoubleProvider alphaProvider; - - private final FrameTuple3DReadOnly position; - private final YoBoolean hasBeenCalled; - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFramePoint(String, String, YoRegistry, double, ReferenceFrame)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, - ReferenceFrame referenceFrame) - { - return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, referenceFrame); - } - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFramePoint(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, - DoubleProvider alpha, ReferenceFrame referenceFrame) - { - return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, referenceFrame); - } - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFramePoint(String, String, YoRegistry, double, FrameTuple3DReadOnly)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, - FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); - } - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFramePoint(String, String, YoRegistry, DoubleProvider, FrameTuple3DReadOnly)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, - DoubleProvider alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - return new AlphaFilteredYoFramePoint(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); - } - - public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); - } - - public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); - } - - public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, - FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), - unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); - } - - public AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, - FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); - } - - private AlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, - FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - alphaProvider = alpha; - - position = unfilteredFrameTuple3D; - hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (position == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(position); - } - - public void update(FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - checkReferenceFrameMatch(unfilteredFrameTuple3D); - update((Tuple3DReadOnly) unfilteredFrameTuple3D); - } - - public void update(Tuple3DReadOnly unfilteredTuple3D) - { - update(unfilteredTuple3D.getX(), unfilteredTuple3D.getY(), unfilteredTuple3D.getZ()); - } - - private final Point3D unfilteredPoint3D = new Point3D(); - - public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) - { - if (!hasBeenCalled.getValue()) - { - hasBeenCalled.set(true); - set(xUnfiltered, yUnfiltered, zUnfiltered); - } - else - { - unfilteredPoint3D.set(xUnfiltered, yUnfiltered, zUnfiltered); - interpolate(unfilteredPoint3D, this, alphaProvider.getValue()); - } - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java deleted file mode 100644 index 3cf1ff84..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePoint2d.java +++ /dev/null @@ -1,156 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; -import us.ihmc.euclid.tuple2D.Point2D; -import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; - -public class AlphaFilteredYoFramePoint2d extends YoFramePoint2D -{ - private final DoubleProvider alphaProvider; - - private final FrameTuple2DReadOnly position; - private final YoBoolean hasBeenCalled; - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFramePoint2d(String, String, YoRegistry, double, ReferenceFrame)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, - ReferenceFrame referenceFrame) - { - return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); - } - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFramePoint2d(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, - DoubleProvider alpha, ReferenceFrame referenceFrame) - { - return createAlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); - } - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFramePoint2d(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, String description, - YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) - { - return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); - } - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFramePoint2d(String, String, YoRegistry, double, FrameTuple2DReadOnly)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, - FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); - } - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFramePoint2d(String, String, YoRegistry, DoubleProvider, FrameTuple2DReadOnly)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFramePoint2d createAlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, - DoubleProvider alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - return new AlphaFilteredYoFramePoint2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); - } - - public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); - } - - public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); - } - - public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, - FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), - unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); - } - - public AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, - FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); - } - - private AlphaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, - FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - alphaProvider = alpha; - - position = unfilteredFrameTuple2D; - hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (position == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(position); - } - - public void update(FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - checkReferenceFrameMatch(unfilteredFrameTuple2D); - update((Tuple2DReadOnly) unfilteredFrameTuple2D); - } - - public void update(Tuple2DReadOnly unfilteredTuple2D) - { - update(unfilteredTuple2D.getX(), unfilteredTuple2D.getY()); - } - - private final Point2D unfilteredPoint2D = new Point2D(); - - public void update(double xUnfiltered, double yUnfiltered) - { - if (!hasBeenCalled.getValue()) - { - hasBeenCalled.set(true); - set(xUnfiltered, yUnfiltered); - } - else - { - unfilteredPoint2D.set(xUnfiltered, yUnfiltered); - interpolate(unfilteredPoint2D, this, alphaProvider.getValue()); - } - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java deleted file mode 100644 index 61171278..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFramePose3D.java +++ /dev/null @@ -1,128 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.geometry.Pose3D; -import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePose3D; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.tools.YoGeometryNameTools; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class AlphaFilteredYoFramePose3D extends YoFramePose3D implements ProcessingYoVariable -{ - private final DoubleProvider alpha; - private final FramePose3DReadOnly unfilteredPose; - private final YoBoolean hasBeenCalled; - - private final Pose3D poseMeasured = new Pose3D(); - private final Pose3D posePreviousFiltered = new Pose3D(); - - private static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "AlphaVariable", registry); - maxRate.set(initialValue); - return maxRate; - } - - public AlphaFilteredYoFramePose3D(String namePrefix, String nameSuffix, FramePose3DReadOnly unfilteredPose, double alpha, YoRegistry registry) - { - this(namePrefix, nameSuffix, unfilteredPose, createAlphaYoDouble(namePrefix, alpha, registry), registry); - } - - public AlphaFilteredYoFramePose3D(String namePrefix, String nameSuffix, DoubleProvider alpha, ReferenceFrame referenceFrame, YoRegistry registry) - { - this(namePrefix, nameSuffix, null, alpha, referenceFrame, registry); - } - - public AlphaFilteredYoFramePose3D(String namePrefix, String nameSuffix, FramePose3DReadOnly unfilteredPose, DoubleProvider alpha, YoRegistry registry) - { - this(namePrefix, nameSuffix, unfilteredPose, alpha, unfilteredPose.getReferenceFrame(), registry); - } - - private AlphaFilteredYoFramePose3D(String namePrefix, - String nameSuffix, - FramePose3DReadOnly unfilteredPose, - DoubleProvider alpha, - ReferenceFrame referenceFrame, - YoRegistry registry) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - this.unfilteredPose = unfilteredPose; - - if (alpha == null) - alpha = createAlphaYoDouble(namePrefix, 0.0, registry); - this.alpha = alpha; - - this.hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); - } - - public AlphaFilteredYoFramePose3D(YoFramePoint3D position, - YoFrameQuaternion orientation, - FramePose3DReadOnly unfilteredPose, - DoubleProvider alpha, - YoRegistry registry) - { - super(position, orientation); - this.unfilteredPose = unfilteredPose; - - String namePrefix = YoGeometryNameTools.getCommonPrefix(position.getNamePrefix(), orientation.getNamePrefix()); - String nameSuffix = YoGeometryNameTools.getCommonSuffix(position.getNameSuffix(), orientation.getNameSuffix()); - - if (alpha == null) - alpha = createAlphaYoDouble(namePrefix, 0.0, registry); - this.alpha = alpha; - - this.hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); - } - - @Override - public void update() - { - if (unfilteredPose == null) - { - throw new NullPointerException( - "AlphaFilteredYoFramePose3D must be constructed with a non null " + "pose variable to call update(), otherwise use update(Pose3DReadOnly)"); - } - - poseMeasured.set(unfilteredPose); - update(poseMeasured); - } - - public void update(FramePose3DReadOnly rawPose) - { - checkReferenceFrameMatch(rawPose); - poseMeasured.set(rawPose); - update(poseMeasured); - } - - public void update(Pose3DReadOnly rawPose) - { - if (hasBeenCalled.getBooleanValue()) - { - posePreviousFiltered.set(this); - - interpolate(poseMeasured, posePreviousFiltered, alpha.getValue()); // qPreviousFiltered 'gets multiplied by alpha' - } - else - { - set(poseMeasured); - hasBeenCalled.set(true); - } - } - - @Override - public void reset() - { - hasBeenCalled.set(false); - } - - public FramePose3DReadOnly getUnfilteredPose() - { - return unfilteredPose; - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java deleted file mode 100644 index d19a7b04..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameQuaternion.java +++ /dev/null @@ -1,113 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; -import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class AlphaFilteredYoFrameQuaternion extends YoFrameQuaternion implements ProcessingYoVariable -{ - private final YoFrameQuaternion unfilteredQuaternion; - private final DoubleProvider alpha; - private final YoBoolean hasBeenCalled; - private final Quaternion qMeasured = new Quaternion(); - private final Quaternion qPreviousFiltered = new Quaternion(); - private final Quaternion qNewFiltered = new Quaternion(); - - private static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "AlphaVariable", registry); - maxRate.set(initialValue); - return maxRate; - } - - public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, double alpha, - YoRegistry registry) - { - this(namePrefix, nameSuffix, unfilteredQuaternion, createAlphaYoDouble(namePrefix, alpha, registry), registry); - } - - public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, DoubleProvider alpha, ReferenceFrame referenceFrame, - YoRegistry registry) - { - this(namePrefix, nameSuffix, null, alpha, referenceFrame, registry); - } - - public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, DoubleProvider alpha, - YoRegistry registry) - { - this(namePrefix, nameSuffix, unfilteredQuaternion, alpha, unfilteredQuaternion.getReferenceFrame(), registry); - } - - private AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, DoubleProvider alpha, - ReferenceFrame referenceFrame, YoRegistry registry) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - this.unfilteredQuaternion = unfilteredQuaternion; - - if (alpha == null) - alpha = createAlphaYoDouble(namePrefix, 0.0, registry); - this.alpha = alpha; - - this.hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); - } - - @Override - public void update() - { - if (unfilteredQuaternion == null) - { - throw new NullPointerException("AlphaFilteredYoFrameQuaternion must be constructed with a non null " - + "quaternion variable to call update(), otherwise use update(Quat4d)"); - } - - qMeasured.set(unfilteredQuaternion); - update(qMeasured); - } - - public void update(FrameOrientation3DReadOnly rawOrientation) - { - checkReferenceFrameMatch(rawOrientation); - qMeasured.set(rawOrientation); - update(qMeasured); - } - - public void update(Orientation3DReadOnly rawOrientation) - { - qMeasured.set(rawOrientation); - update(qMeasured); - } - - private void update(QuaternionReadOnly qMeasured) - { - if (hasBeenCalled.getBooleanValue()) - { - qPreviousFiltered.set(this); - - qNewFiltered.interpolate(qMeasured, qPreviousFiltered, alpha.getValue()); // qPreviousFiltered 'gets multiplied by alpha' - set(qNewFiltered); - } - else - { - set(qMeasured); - hasBeenCalled.set(true); - } - } - - @Override - public void reset() - { - hasBeenCalled.set(false); - } - - public YoFrameQuaternion getUnfilteredQuaternion() - { - return unfilteredQuaternion; - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java deleted file mode 100644 index dcb4bba8..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector.java +++ /dev/null @@ -1,146 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; - -public class AlphaFilteredYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable -{ - private final DoubleProvider alphaProvider; - - private final FrameTuple3DReadOnly position; - private final YoBoolean hasBeenCalled; - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFrameVector(String, String, YoRegistry, double, ReferenceFrame)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, - ReferenceFrame referenceFrame) - { - return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, referenceFrame); - } - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFrameVector(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, - DoubleProvider alpha, ReferenceFrame referenceFrame) - { - return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, referenceFrame); - } - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFrameVector(String, String, YoRegistry, double, FrameTuple3DReadOnly)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, - FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); - } - - /** - * @deprecated Use - * {@link #AlphaFilteredYoFrameVector(String, String, YoRegistry, DoubleProvider, FrameTuple3DReadOnly)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, - DoubleProvider alpha, FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - return new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D); - } - - public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); - } - - public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); - } - - public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, - FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), - unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); - } - - public AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, - FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); - } - - private AlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, - FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - alphaProvider = alpha; - - position = unfilteredFrameTuple3D; - hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); - reset(); - } - - @Override - public void reset() - { - hasBeenCalled.set(false); - } - - @Override - public void update() - { - if (position == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(position); - } - - public void update(FrameTuple3DReadOnly unfilteredFrameTuple3D) - { - checkReferenceFrameMatch(unfilteredFrameTuple3D); - update((Tuple3DReadOnly) unfilteredFrameTuple3D); - } - - public void update(Tuple3DReadOnly unfilteredTuple3D) - { - update(unfilteredTuple3D.getX(), unfilteredTuple3D.getY(), unfilteredTuple3D.getZ()); - } - - private final Vector3D unfilteredVector3D = new Vector3D(); - - public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) - { - if (!hasBeenCalled.getValue()) - { - hasBeenCalled.set(true); - set(xUnfiltered, yUnfiltered, zUnfiltered); - } - else - { - unfilteredVector3D.set(xUnfiltered, yUnfiltered, zUnfiltered); - interpolate(unfilteredVector3D, this, alphaProvider.getValue()); - } - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java deleted file mode 100644 index 179797e8..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoFrameVector2d.java +++ /dev/null @@ -1,170 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; -import us.ihmc.euclid.tuple2D.Vector2D; -import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; - -public class AlphaFilteredYoFrameVector2d extends YoFrameVector2D -{ - private final DoubleProvider alphaProvider; - - private final FrameTuple2DReadOnly position; - private final YoBoolean hasBeenCalled; - - /** - * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, double, ReferenceFrame)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, - double alpha, ReferenceFrame referenceFrame) - { - return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); - } - - /** - * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, - DoubleProvider alpha, ReferenceFrame referenceFrame) - { - return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, referenceFrame); - } - - /** - * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, DoubleProvider, ReferenceFrame)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, - YoRegistry registry, DoubleProvider alpha, - ReferenceFrame referenceFrame) - { - return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, referenceFrame); - } - - /** - * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, double, FrameTuple2DReadOnly)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, - double alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); - } - - /** - * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, DoubleProvider, FrameTuple2DReadOnly)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, - DoubleProvider alpha, FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - return createAlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, "", registry, alpha, unfilteredFrameTuple2D); - } - - /** - * @deprecated Use - * {@link #createAlphaFilteredYoFrameVector2d(String, String, YoRegistry, DoubleProvider, FrameTuple2DReadOnly)} - * instead. - */ - @Deprecated - public static AlphaFilteredYoFrameVector2d createAlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, String description, - YoRegistry registry, DoubleProvider alpha, - FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - return new AlphaFilteredYoFrameVector2d(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D); - } - - public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), referenceFrame); - } - - public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); - } - - public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, - FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - this(namePrefix, nameSuffix, registry, AlphaFilteredYoVariable.createAlphaYoDouble(namePrefix + nameSuffix, alpha, registry), - unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); - } - - public AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, - FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); - } - - private AlphaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, - FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - alphaProvider = alpha; - - position = unfilteredFrameTuple2D; - hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (position == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(position); - } - - public void update(FrameTuple2DReadOnly unfilteredFrameTuple2D) - { - checkReferenceFrameMatch(unfilteredFrameTuple2D); - update((Tuple2DReadOnly) unfilteredFrameTuple2D); - } - - public void update(Tuple2DReadOnly unfilteredTuple2D) - { - update(unfilteredTuple2D.getX(), unfilteredTuple2D.getY()); - } - - private final Vector2D unfilteredVector2D = new Vector2D(); - - public void update(double xUnfiltered, double yUnfiltered) - { - if (!hasBeenCalled.getValue()) - { - hasBeenCalled.set(true); - set(xUnfiltered, yUnfiltered); - } - else - { - unfilteredVector2D.set(xUnfiltered, yUnfiltered); - interpolate(unfilteredVector2D, this, alphaProvider.getValue()); - } - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java deleted file mode 100644 index 296f315c..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMatrix.java +++ /dev/null @@ -1,109 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import org.ejml.data.DMatrix; -import org.ejml.data.DMatrixRMaj; -import org.ejml.dense.row.CommonOps_DDRM; -import us.ihmc.robotics.math.frames.YoMatrix; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; - -public class AlphaFilteredYoMatrix extends YoMatrix -{ - private final DMatrixRMaj previous; - private final DMatrixRMaj current; - private final DMatrixRMaj filtered; - - private final YoDouble alpha; - - public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, YoRegistry registry) - { - this(name, null, alpha, numberOfRows, numberOfColumns, null, null, registry); - } - - public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, YoRegistry registry) - { - this(name, null, alpha, numberOfRows, numberOfColumns, rowNames, null, registry); - } - - public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) - { - this(name, null, alpha, numberOfRows, numberOfColumns, rowNames, columnNames, registry); - } - - public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, YoRegistry registry) - { - this(name, description, alpha, numberOfRows, numberOfColumns, null, null, registry); - } - - public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, YoRegistry registry) - { - this(name, description, alpha, numberOfRows, numberOfColumns, rowNames, null, registry); - } - - public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) - { - super(name, description, numberOfRows, numberOfColumns, rowNames, columnNames, registry); - this.alpha = new YoDouble(name + "_alpha", registry); - this.alpha.set(alpha); - - previous = new DMatrixRMaj(numberOfRows, numberOfColumns); - current = new DMatrixRMaj(numberOfRows, numberOfColumns); - filtered = new DMatrixRMaj(numberOfRows, numberOfColumns); - } - - public void setAlpha(double alpha) - { - this.alpha.set(alpha); - } - - /** - * Set the current value of the matrix to be filtered. - *

- * NOTE: This method does not solve for the filtered value. To solve for the filtered value, use {@link #solve()}. - *

- * - * @param current the current value of the matrix to be filtered. Not modified. - */ - @Override - public void set(DMatrix current) - { - super.set(current); - this.current.set(current); - } - - /** - * Assuming that the current value has been set, this method solves for the filtered value. - *

- * See {@link #set(DMatrix)} for how to set the matrix's current value. - *

- */ - public void solve() - { - CommonOps_DDRM.scale(alpha.getDoubleValue(), previous, filtered); - - super.get(current); - CommonOps_DDRM.addEquals(filtered, 1 - alpha.getDoubleValue(), current); - - // Set the previous value to be the output of the filter, so it can be used next time - previous.set(filtered); - super.set(filtered); - } - - /** - * Set the current value of the matrix to be filtered and solve for the filtered value. - * - * @param current the current value of the matrix to be filtered. Not modified. - */ - public void setAndSolve(DMatrix current) - { - CommonOps_DDRM.scale(alpha.getDoubleValue(), previous, filtered); - - super.set(current); - this.current.set(current); - CommonOps_DDRM.addEquals(filtered, 1 - alpha.getDoubleValue(), this.current); - - // Set the previous value to be the output of the filter, so it can be used next time - previous.set(filtered); - super.set(filtered); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java deleted file mode 100644 index 6da82cc5..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoVariable.java +++ /dev/null @@ -1,181 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.commons.MathTools; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -/** - * @author jrebula - *

- *

- * LittleDogVersion06: - * us.ihmc.LearningLocomotion.Version06.util.YoAlphaFilteredVariable, - * 9:34:00 AM, Aug 29, 2006 - *

= - *

- * A YoAlphaFilteredVariable is a filtered version of an input YoVar. - * Either a YoVariable holding the unfiltered val is passed in to the - * constructor and update() is called every tick, or update(double) is - * called every tick. The YoAlphaFilteredVariable updates it's val - * with the current filtered version using - *

- * 
- *            filtered_{n} = alpha * filtered_{n-1} + (1 - alpha) * raw_{n}
- *
- * - * For alpha=0 -> no filtered - * For alpha=1 -> 100% filtered, no use of raw signal - */ -public class AlphaFilteredYoVariable extends YoDouble implements ProcessingYoVariable -{ - private final DoubleProvider alphaVariable; - - private final YoDouble position; - protected final YoBoolean hasBeenCalled; - - public static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "AlphaVariable", registry); - maxRate.set(initialValue); - return maxRate; - } - - public AlphaFilteredYoVariable(String name, YoRegistry registry, double alpha) - { - this(name, registry, alpha, null); - } - - public AlphaFilteredYoVariable(String name, YoRegistry registry, double alpha, YoDouble positionVariable) - { - this(name, "", registry, createAlphaYoDouble(name, alpha, registry), positionVariable); - } - - public AlphaFilteredYoVariable(String name, YoRegistry registry, DoubleProvider alphaVariable) - { - this(name, "", registry, alphaVariable, null); - } - - public AlphaFilteredYoVariable(String name, String description, YoRegistry registry, DoubleProvider alphaVariable) - { - this(name, description, registry, alphaVariable, null); - } - - public AlphaFilteredYoVariable(String name, YoRegistry registry, DoubleProvider alphaVariable, YoDouble positionVariable) - { - this(name, "", registry, alphaVariable, positionVariable); - } - - public AlphaFilteredYoVariable(String name, String description, YoRegistry 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; - if(alphaVariable instanceof YoDouble) - { - this.yoAlphaVariable = (YoDouble) alphaVariable; - } - else - { - this.yoAlphaVariable = null; - } - - reset(); - } - - @Override - public void reset() - { - hasBeenCalled.set(false); - } - - @Override - public void update() - { - if (position == null) - { - throw new NullPointerException("YoAlphaFilteredVariable must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(position.getDoubleValue()); - } - - public void update(double currentPosition) - { - if (!hasBeenCalled.getBooleanValue()) - { - hasBeenCalled.set(true); - set(currentPosition); - } - else - { - double alpha = alphaVariable.getValue(); - set(alpha * getDoubleValue() + (1.0 - alpha) * currentPosition); - } - } - - /** - * This method is replaced by computeAlphaGivenBreakFrequencyProperly. It is fine to keep using this method is currently using it, knowing that - * the actual break frequency is not exactly what you are asking for. - * - * @param breakFrequencyInHertz - * @param dt - * @return - */ - @Deprecated - public static double computeAlphaGivenBreakFrequency(double breakFrequencyInHertz, double dt) - { - if (Double.isInfinite(breakFrequencyInHertz)) - return 0.0; - - double alpha = 1.0 - breakFrequencyInHertz * 2.0 * Math.PI * dt; - - alpha = MathTools.clamp(alpha, 0.0, 1.0); - - return alpha; - } - - public static double computeAlphaGivenBreakFrequencyProperly(double breakFrequencyInHertz, double dt) - { - if (Double.isInfinite(breakFrequencyInHertz)) - return 0.0; - - double omega = 2.0 * Math.PI * breakFrequencyInHertz; - double alpha = (1.0 - omega * dt / 2.0) / (1.0 + omega * dt / 2.0); - alpha = MathTools.clamp(alpha, 0.0, 1.0); - return alpha; - } - - public static double computeBreakFrequencyGivenAlpha(double alpha, double dt) - { - return (1.0 - alpha) / (Math.PI * dt * (1.0 + alpha)); - } - - 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 boolean getHasBeenCalled() - { - return hasBeenCalled.getBooleanValue(); - } - -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoDouble.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoDouble.java deleted file mode 100644 index d42f5774..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoDouble.java +++ /dev/null @@ -1,52 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoInteger; - -public class AverageYoDouble extends YoDouble -{ - private final YoInteger sampleSize; - private final DoubleProvider dataSource; - - public AverageYoDouble(String name, YoRegistry registry) - { - this(name, null, registry); - } - - public AverageYoDouble(String name, DoubleProvider dataSource, YoRegistry registry) - { - super(name, registry); - - this.dataSource = dataSource; - sampleSize = new YoInteger(name + "SampleSize", registry); - } - - public void update() - { - if (dataSource == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "dataSource variable to call update(), otherwise use update(double)"); - } - - update(dataSource.getValue()); - } - - public void update(double dataSource) - { - sampleSize.increment(); - add((dataSource - getValue()) / sampleSize.getValue()); - } - - public void reset() - { - sampleSize.set(0); - } - - public int getSampleSize() - { - return sampleSize.getValue(); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoFrameVector3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoFrameVector3D.java deleted file mode 100644 index d7197689..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AverageYoFrameVector3D.java +++ /dev/null @@ -1,78 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.tools.YoGeometryNameTools; -import us.ihmc.yoVariables.variable.YoInteger; - -public class AverageYoFrameVector3D extends YoFrameVector3D -{ - private final YoInteger sampleSize; - private final Tuple3DReadOnly dataSource; - - public AverageYoFrameVector3D(String namePrefix, ReferenceFrame referenceFrame, YoRegistry registry) - { - this(namePrefix, "", registry, referenceFrame, null); - } - - public AverageYoFrameVector3D(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, YoRegistry registry) - { - this(namePrefix, nameSuffix, registry, referenceFrame, null); - } - - public AverageYoFrameVector3D(String namePrefix, YoRegistry registry, FrameTuple3DReadOnly dataSource) - { - this(namePrefix, "", registry, dataSource.getReferenceFrame(), dataSource); - } - - public AverageYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, FrameTuple3DReadOnly dataSource) - { - this(namePrefix, nameSuffix, registry, dataSource.getReferenceFrame(), dataSource); - } - - private AverageYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, ReferenceFrame referenceFrame, Tuple3DReadOnly dataSource) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - this.dataSource = dataSource; - - sampleSize = new YoInteger(YoGeometryNameTools.assembleName(namePrefix, "sampleSize", nameSuffix), registry); - } - - public void update() - { - update(dataSource); - } - - public void update(Tuple3DReadOnly vectorSource) - { - update(vectorSource.getX(), vectorSource.getY(), vectorSource.getZ()); - } - - public void update(FrameTuple3DReadOnly vectorSource) - { - checkReferenceFrameMatch(vectorSource); - update((Tuple3DReadOnly) vectorSource); - } - - public void update(double xSource, double ySource, double zSource) - { - sampleSize.increment(); - setX(getX() + (xSource - getX()) / sampleSize.getValue()); - setY(getY() + (ySource - getY()) / sampleSize.getValue()); - setZ(getZ() + (zSource - getZ()) / sampleSize.getValue()); - } - - public void reset() - { - sampleSize.set(0); - } - - public int getSampleSize() - { - return sampleSize.getValue(); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java deleted file mode 100644 index ce05ef8b..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoFrameVector.java +++ /dev/null @@ -1,77 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.tools.YoGeometryNameTools; -import us.ihmc.yoVariables.variable.YoDouble; - -public class BacklashCompensatingVelocityYoFrameVector extends YoFrameVector3D -{ - private final BacklashCompensatingVelocityYoVariable xDot, yDot, zDot; - - public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensatingVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, YoDouble slopTime, - YoRegistry registry, YoFrameVector3D yoFrameVectorToDifferentiate) - { - BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoX(), dt, slopTime, - registry); - BacklashCompensatingVelocityYoVariable yDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoY(), dt, slopTime, - registry); - BacklashCompensatingVelocityYoVariable zDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFrameVectorToDifferentiate.getYoZ(), dt, slopTime, - registry); - - BacklashCompensatingVelocityYoFrameVector ret = new BacklashCompensatingVelocityYoFrameVector(xDot, yDot, zDot, registry, yoFrameVectorToDifferentiate); - - return ret; - } - - public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensatingVelocityYoFrameVector(String namePrefix, String nameSuffix, YoDouble alpha, double dt, YoDouble slopTime, - YoRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) - { - BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoX(), dt, slopTime, - registry); - BacklashCompensatingVelocityYoVariable yDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoY(), dt, slopTime, - registry); - BacklashCompensatingVelocityYoVariable zDot = new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoZ(), dt, slopTime, - registry); - - BacklashCompensatingVelocityYoFrameVector ret = new BacklashCompensatingVelocityYoFrameVector(xDot, yDot, zDot, registry, yoFramePointToDifferentiate); - - return ret; - } - - private BacklashCompensatingVelocityYoFrameVector(BacklashCompensatingVelocityYoVariable xDot, BacklashCompensatingVelocityYoVariable yDot, BacklashCompensatingVelocityYoVariable zDot, - YoRegistry registry, YoFrameVector3D yoFrameVectorToDifferentiate) - { - super(xDot, yDot, zDot, yoFrameVectorToDifferentiate.getReferenceFrame()); - - this.xDot = xDot; - this.yDot = yDot; - this.zDot = zDot; - } - - private BacklashCompensatingVelocityYoFrameVector(BacklashCompensatingVelocityYoVariable xDot, BacklashCompensatingVelocityYoVariable yDot, BacklashCompensatingVelocityYoVariable zDot, - YoRegistry registry, YoFramePoint3D yoFramePointToDifferentiate) - { - super(xDot, yDot, zDot, yoFramePointToDifferentiate.getReferenceFrame()); - - this.xDot = xDot; - this.yDot = yDot; - this.zDot = zDot; - } - - public void update() - { - xDot.update(); - yDot.update(); - zDot.update(); - } - - public void reset() - { - xDot.reset(); - yDot.reset(); - zDot.reset(); - } - -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java deleted file mode 100644 index 81354164..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashCompensatingVelocityYoVariable.java +++ /dev/null @@ -1,242 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.commons.MathTools; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoEnum; - -public class BacklashCompensatingVelocityYoVariable extends YoDouble implements ProcessingYoVariable -{ - private final double dt; - - private final YoDouble alphaVariable; - private final YoDouble position; - - private final YoDouble lastPosition; - private final YoBoolean hasBeenCalled; - - private final YoEnum backlashState; - private final YoDouble slopTime; - - private final YoDouble timeInState; - - public BacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, YoDouble positionVariable, double dt, - YoDouble slopTime, YoRegistry registry) - { - super(name, description, registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - - backlashState = new YoEnum(name + "BacklashState", registry, BacklashState.class, true); - backlashState.set(null); - timeInState = new YoDouble(name + "TimeInState", registry); - - position = positionVariable; - - this.alphaVariable = alphaVariable; - this.slopTime = slopTime; - - this.dt = dt; - - lastPosition = new YoDouble(name + "_lastPosition", registry); - - reset(); - } - - public BacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, double dt, YoDouble slopTime, - YoRegistry registry) - { - super(name, description, registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - - backlashState = new YoEnum(name + "BacklashState", registry, BacklashState.class, true); - backlashState.set(null); - timeInState = new YoDouble(name + "timeInState", registry); - - this.position = null; - - this.alphaVariable = alphaVariable; - this.slopTime = slopTime; - - this.dt = dt; - - lastPosition = new YoDouble(name + "_lastPosition", registry); - - reset(); - } - - @Override - public void reset() - { - hasBeenCalled.set(false); - backlashState.set(null); - } - - @Override - public void update() - { - if (position == null) - { - throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(position.getDoubleValue()); - } - - // public void updateForAngles() - // { - // if (position == null) - // { - // throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " - // + "position variable to call update(), otherwise use update(double)"); - // } - // - // updateForAngles(position.getDoubleValue()); - // } - - public void update(double currentPosition) - { - if (!hasBeenCalled.getBooleanValue()) - { - hasBeenCalled.set(true); - lastPosition.set(currentPosition); - set(0.0); - } - - timeInState.add(dt); - - boolean sloppy = false; - - double difference = currentPosition - lastPosition.getDoubleValue(); - - if (backlashState.getEnumValue() == null) - { - if (difference > 0.0) - backlashState.set(BacklashState.FORWARD_OK); - else if (difference < 0.0) - backlashState.set(BacklashState.BACKWARD_OK); - } - else - { - switch (backlashState.getEnumValue()) - { - case BACKWARD_OK: - { - if (difference > 0.0) - { - backlashState.set(BacklashState.FORWARD_SLOP); - sloppy = true; - timeInState.set(0.0); - } - break; - } - case FORWARD_OK: - { - if (difference < 0.0) - { - backlashState.set(BacklashState.BACKWARD_SLOP); - sloppy = true; - timeInState.set(0.0); - } - - break; - } - case BACKWARD_SLOP: - { - sloppy = true; - - if (difference > 0.0) - { - backlashState.set(BacklashState.FORWARD_SLOP); - sloppy = true; - timeInState.set(0.0); - } - else if (timeInState.getDoubleValue() > slopTime.getDoubleValue()) - { - backlashState.set(BacklashState.BACKWARD_OK); - sloppy = false; - timeInState.set(0.0); - } - - break; - } - case FORWARD_SLOP: - { - sloppy = true; - - if (difference < 0.0) - { - backlashState.set(BacklashState.BACKWARD_SLOP); - sloppy = true; - timeInState.set(0.0); - } - else if (timeInState.getDoubleValue() > slopTime.getDoubleValue()) - { - backlashState.set(BacklashState.FORWARD_OK); - sloppy = false; - timeInState.set(0.0); - } - break; - } - } - } - - if (sloppy) - { - double percent = timeInState.getDoubleValue() / slopTime.getDoubleValue(); - percent = MathTools.clamp(percent, 0.0, 1.0); - if (Double.isNaN(percent)) - percent = 1.0; - - double scaleFactor = percent; - - difference = scaleFactor * difference; - } - - updateUsingDifference(difference); - - lastPosition.set(currentPosition); - - } - - // public void updateForAngles(double currentPosition) - // { - // if (!hasBeenCalled.getBooleanValue()) - // { - // hasBeenCalled.set(true); - // lastPosition.set(currentPosition); - // set(0.0); - // } - // - // double difference = AngleTools.computeAngleDifferenceMinusPiToPi(currentPosition, lastPosition.getDoubleValue()); - // - // updateUsingDifference(difference); - // - // lastPosition.set(currentPosition); - // } - - private void updateUsingDifference(double difference) - { - double previousFilteredDerivative = getDoubleValue(); - double currentRawDerivative = difference / dt; - - double alpha = alphaVariable.getDoubleValue(); - set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); - } - - public void setAlpha(double alpha) - { - this.alphaVariable.set(alpha); - } - - public void setSlopTime(double slopTime) - { - this.slopTime.set(slopTime); - } - - private enum BacklashState - { - BACKWARD_OK, FORWARD_OK, BACKWARD_SLOP, FORWARD_SLOP; - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java deleted file mode 100644 index 85488225..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BacklashProcessingYoVariable.java +++ /dev/null @@ -1,153 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.commons.MathTools; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoEnum; - -/** - * This does essentially the same as RevisedBacklashCompensatingVelocityYoVariable, except it takes a velocity signal as input. - * - */ -public class BacklashProcessingYoVariable extends YoDouble implements ProcessingYoVariable -{ - private final YoDouble velocity; - - private final YoBoolean hasBeenCalled; - - private final YoEnum backlashState; - private final DoubleProvider slopTime; - - private final YoDouble timeSinceSloppy; - - private final double dt; - - public BacklashProcessingYoVariable(String name, String description, double dt, DoubleProvider slopTime, YoRegistry registry) - { - this(name, description, null, dt, slopTime, registry); - } - - public BacklashProcessingYoVariable(String name, String description, YoDouble velocityVariable, double dt, DoubleProvider slopTime, - YoRegistry registry) - { - super(name, description, registry); - - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - - backlashState = new YoEnum(name + "BacklashState", registry, BacklashState.class, true); - backlashState.set(null); - timeSinceSloppy = new YoDouble(name + "TimeSinceSloppy", registry); - - velocity = velocityVariable; - - this.slopTime = slopTime; - - this.dt = dt; - - reset(); - } - - @Override - public void reset() - { - hasBeenCalled.set(false); - backlashState.set(null); - } - - @Override - public void update() - { - if (velocity == null) - { - throw new NullPointerException( - "BacklashProcessingYoVariable must be constructed with a non null " + "velocity variable to call update(), otherwise use update(double)"); - } - - update(velocity.getDoubleValue()); - } - - public void update(double currentVelocity) - { - if (backlashState.getEnumValue() == null) - { - backlashState.set(BacklashState.FORWARD_OK); - } - - if (!hasBeenCalled.getBooleanValue()) - { - hasBeenCalled.set(true); - set(currentVelocity); - } - - timeSinceSloppy.add(dt); - - switch (backlashState.getEnumValue()) - { - case BACKWARD_OK: - { - if (currentVelocity > 0.0) - { - timeSinceSloppy.set(0.0); - backlashState.set(BacklashState.FORWARD_SLOP); - } - - break; - } - - case FORWARD_OK: - { - if (currentVelocity < 0.0) - { - timeSinceSloppy.set(0.0); - backlashState.set(BacklashState.BACKWARD_SLOP); - } - - break; - } - - case BACKWARD_SLOP: - { - if (currentVelocity > 0.0) - { - timeSinceSloppy.set(0.0); - backlashState.set(BacklashState.FORWARD_SLOP); - } - else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) - { - backlashState.set(BacklashState.BACKWARD_OK); - } - - break; - } - - case FORWARD_SLOP: - { - if (currentVelocity < 0.0) - { - timeSinceSloppy.set(0.0); - backlashState.set(BacklashState.BACKWARD_SLOP); - } - else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) - { - backlashState.set(BacklashState.FORWARD_OK); - } - - break; - } - } - - double percent = timeSinceSloppy.getDoubleValue() / slopTime.getValue(); - percent = MathTools.clamp(percent, 0.0, 1.0); - if (Double.isNaN(percent) || slopTime.getValue() < dt) - percent = 1.0; - - this.set(percent * currentVelocity); - } - - private enum BacklashState - { - BACKWARD_OK, FORWARD_OK, BACKWARD_SLOP, FORWARD_SLOP; - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java deleted file mode 100644 index 80bc29b1..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFramePoint2d.java +++ /dev/null @@ -1,85 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.FramePoint2D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.tuple2D.Point2D; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.tools.YoGeometryNameTools; - -public class BetaFilteredYoFramePoint2d extends YoFramePoint2D -{ - private final BetaFilteredYoVariable x, y; - - private BetaFilteredYoFramePoint2d(BetaFilteredYoVariable x, BetaFilteredYoVariable y, ReferenceFrame referenceFrame) - { - super(x, y, referenceFrame); - - this.x = x; - this.y = y; - } - - public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, int beta, ReferenceFrame referenceFrame) - { - // beta is a int - BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta); - BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta); - - BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, referenceFrame); - - return ret; - } - - public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, String description, YoRegistry registry, int beta, ReferenceFrame referenceFrame) - { - // beta is a int - BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), description, registry, beta); - BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), description, registry, beta); - - BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, referenceFrame); - - return ret; - } - - public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoRegistry registry, int beta, YoFramePoint2D unfilteredPoint) - { - // beta is a int - BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoX()); - BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoY()); - - BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, unfilteredPoint.getReferenceFrame()); - - return ret; - } - - public void update() - { - x.update(); - y.update(); - } - - public void update(double xUnfiltered, double yUnfiltered) - { - x.update(xUnfiltered); - y.update(yUnfiltered); - } - - public void update(Point2D point2dUnfiltered) - { - x.update(point2dUnfiltered.getX()); - y.update(point2dUnfiltered.getY()); - } - - public void update(FramePoint2D point2dUnfiltered) - { - checkReferenceFrameMatch(point2dUnfiltered); - x.update(point2dUnfiltered.getX()); - y.update(point2dUnfiltered.getY()); - } - - public void reset() - { - x.reset(); - y.reset(); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java deleted file mode 100644 index f65b2beb..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoFrameVector2d.java +++ /dev/null @@ -1,75 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.FrameVector2D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.tuple2D.Vector2D; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.tools.YoGeometryNameTools; - -public class BetaFilteredYoFrameVector2d extends YoFrameVector2D -{ - private final BetaFilteredYoVariable x, y; - - private BetaFilteredYoFrameVector2d(BetaFilteredYoVariable x, BetaFilteredYoVariable y, ReferenceFrame referenceFrame) - { - super(x, y, referenceFrame); - - this.x = x; - this.y = y; - } - - public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, int beta, ReferenceFrame referenceFrame) - { - // beta is a int - BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta); - BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta); - - BetaFilteredYoFrameVector2d ret = new BetaFilteredYoFrameVector2d(x, y, referenceFrame); - - return ret; - } - - public static BetaFilteredYoFrameVector2d createBetaFilteredYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, int beta, YoFrameVector2D unfilteredVector) - { - // beta is a int - BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoX()); - BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoY()); - - BetaFilteredYoFrameVector2d ret = new BetaFilteredYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); - - return ret; - } - - - public void update() - { - x.update(); - y.update(); - } - - public void update(double xUnfiltered, double yUnfiltered) - { - x.update(xUnfiltered); - y.update(yUnfiltered); - } - - public void update(Vector2D vector2dUnfiltered) - { - x.update(vector2dUnfiltered.getX()); - y.update(vector2dUnfiltered.getY()); - } - - public void update(FrameVector2D vector2dUnfiltered) - { - checkReferenceFrameMatch(vector2dUnfiltered); - x.update(vector2dUnfiltered.getX()); - y.update(vector2dUnfiltered.getY()); - } - - public void reset() - { - x.reset(); - y.reset(); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java deleted file mode 100644 index 4fc3fcc7..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/BetaFilteredYoVariable.java +++ /dev/null @@ -1,105 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -/** - * @author thutcheson - *

- *

- *

- *

- * A BetaFilteredYoVariable is a filtered version of an input YoVar. - * Either a YoVariable holding the unfiltered val is passed in to the - * constructor and update() is called every tick, or update(double) is - * called every tick. The BetaFilteredYoVariable updates it's val - * with the current filtered version using - *

- * 
- *            filtered_{n} = (raw_{0} + ... + raw_{n-1} + raw_{n}) / n
- *
- */ -public class BetaFilteredYoVariable extends YoDouble -{ - private int beta; - private int index = 0; - @SuppressWarnings("unused") - private final YoDouble betaVariable; - - private final YoDouble position; - - private static double raw[]; - - private final YoBoolean hasBeenCalled; - - public BetaFilteredYoVariable(String name, YoRegistry registry, int beta) - { - this(name, "", registry, beta, null); - } - - public BetaFilteredYoVariable(String name, String description, YoRegistry registry, int beta) - { - this(name, description, registry, beta, null); - } - - public BetaFilteredYoVariable(String name, YoRegistry registry, int beta, YoDouble positionVariable) - { - this(name, "", registry, beta, positionVariable); - } - - public BetaFilteredYoVariable(String name, String description, YoRegistry registry, int beta, YoDouble positionVariable) - { - super(name, description, registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - - this.beta = beta; - this.position = positionVariable; - this.betaVariable = null; - - raw = new double[beta]; - - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - - for (int i = 0; i < beta; i++) - { - set(0.0); - } - } - - public void update() - { - if (position == null) - { - throw new NullPointerException("BetaFilteredYoVariable must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(position.getDoubleValue()); - } - public void update(double currentPosition) - { - if (!hasBeenCalled.getBooleanValue()) - { - hasBeenCalled.set(true); - set(currentPosition); - } - - raw[index++] = currentPosition; - if (index == beta) - index = 0; - set(0.0); - - for (int i = 0; i < beta; i++) - { - set(getDoubleValue() + raw[i]); - } - - set(getDoubleValue() / beta); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java deleted file mode 100644 index 419f806b..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoFrameVector.java +++ /dev/null @@ -1,72 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameTuple3D; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.tools.YoGeometryNameTools; -import us.ihmc.yoVariables.variable.YoDouble; - -public class DeadzoneYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable -{ - private final DeadzoneYoVariable x, y, z; - - private DeadzoneYoFrameVector(DeadzoneYoVariable x, DeadzoneYoVariable y, DeadzoneYoVariable z, ReferenceFrame referenceFrame) - { - super(x, y, z, referenceFrame); - - this.x = x; - this.y = y; - this.z = z; - } - - public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, YoRegistry registry, YoDouble deadzoneSize, ReferenceFrame referenceFrame) - { - return createDeadzoneYoFrameVector(namePrefix, "", registry, deadzoneSize, referenceFrame); - } - - public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, YoDouble deadzoneSize, ReferenceFrame referenceFrame) - { - DeadzoneYoVariable x = new DeadzoneYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), deadzoneSize, registry); - DeadzoneYoVariable y = new DeadzoneYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), deadzoneSize, registry); - DeadzoneYoVariable z = new DeadzoneYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), deadzoneSize, registry); - - DeadzoneYoFrameVector ret = new DeadzoneYoFrameVector(x, y, z, referenceFrame); - - return ret; - } - - public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, YoRegistry registry, YoDouble deadzoneSize, YoFrameTuple3D rawTuple) - { - return createDeadzoneYoFrameVector(namePrefix, "", registry, deadzoneSize, rawTuple); - } - - public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, YoDouble deadzoneSize, YoFrameTuple3D rawTuple) - { - DeadzoneYoVariable x = new DeadzoneYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), rawTuple.getYoX(), deadzoneSize, registry); - DeadzoneYoVariable y = new DeadzoneYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), rawTuple.getYoY(), deadzoneSize, registry); - DeadzoneYoVariable z = new DeadzoneYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), rawTuple.getYoZ(), deadzoneSize, registry); - - DeadzoneYoFrameVector ret = new DeadzoneYoFrameVector(x, y, z, rawTuple.getReferenceFrame()); - - return ret; - } - - @Override - public void update() - { - x.update(); - y.update(); - z.update(); - } - - public void update(FrameTuple3DReadOnly frameTuple) - { - checkReferenceFrameMatch(frameTuple); - - x.update(frameTuple.getX()); - y.update(frameTuple.getY()); - z.update(frameTuple.getZ()); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java deleted file mode 100644 index c7e93205..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeadzoneYoVariable.java +++ /dev/null @@ -1,69 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; - -public class DeadzoneYoVariable extends YoDouble implements ProcessingYoVariable -{ - private final YoDouble deadzoneSize; - private final YoDouble inputVariable; - - public DeadzoneYoVariable(String name, YoDouble deadzoneSize, YoRegistry registry) - { - super(name, registry); - this.inputVariable = null; - this.deadzoneSize = deadzoneSize; - } - - public DeadzoneYoVariable(String name, YoDouble inputVariable, YoDouble deadzoneSize, YoRegistry registry) - { - super(name, registry); - this.inputVariable = inputVariable; - this.deadzoneSize = deadzoneSize; - } - - public void update() - { - if (inputVariable == null) - throw new NullPointerException("DeadzoneYoVariable must be constructed with a non null " - + "input variable to call update(), otherwise use update(double)"); - update(inputVariable.getDoubleValue()); - } - - public void update(double valueToBeCorrected) - { - if (valueToBeCorrected >= deadzoneSize.getDoubleValue()) - { - super.set(valueToBeCorrected - deadzoneSize.getDoubleValue()); - } - else if (valueToBeCorrected <= -deadzoneSize.getDoubleValue()) - { - super.set(valueToBeCorrected + deadzoneSize.getDoubleValue()); - } - else - { - super.set(0.0); - } - } - - public static void main(String[] args) - { - YoRegistry registry = new YoRegistry("test"); - YoDouble deadzoneSize = new YoDouble("deadzoneSize", registry); - YoDouble input = new YoDouble("input", registry); - deadzoneSize.set(2.0); - DeadzoneYoVariable testDeadzone = new DeadzoneYoVariable("testDeadZone", input,deadzoneSize,registry); - - for(int i = -50; i < 51; i++) - { - input.set((double)i); - testDeadzone.update(); - System.out.println("//////////////////////////"); - System.out.println("uncorrected = " + (double)i); - System.out.println("corrected = " + testDeadzone.getDoubleValue()); - } - - System.out.println("done"); - } - -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java deleted file mode 100644 index 0d7c554f..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoBoolean.java +++ /dev/null @@ -1,68 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; - -public class DelayedYoBoolean extends YoBoolean -{ - private final YoBoolean variableToDelay; - - private final YoBoolean[] previousYoVariables; - - public DelayedYoBoolean(String name, String description, YoBoolean variableToDelay, int ticksToDelay, YoRegistry registry) - { - super(name, description, registry); - - this.variableToDelay = variableToDelay; - previousYoVariables = new YoBoolean[ticksToDelay]; - - for (int i = 0; i < ticksToDelay; i++) - { - previousYoVariables[i] = new YoBoolean(name + "_previous" + i, registry); - previousYoVariables[i].set(variableToDelay.getBooleanValue()); - } - - this.set(variableToDelay.getBooleanValue()); - } - - public void update() - { - if (previousYoVariables.length == 0) - { - this.set(variableToDelay.getBooleanValue()); - return; - } - - this.set(previousYoVariables[0].getBooleanValue()); - - for (int i = 0; i < previousYoVariables.length - 1; i++) - { - previousYoVariables[i].set(previousYoVariables[i + 1].getBooleanValue()); - } - - previousYoVariables[previousYoVariables.length - 1].set(variableToDelay.getBooleanValue()); - } - - public void reset() - { - for (YoBoolean var : previousYoVariables) - { - var.set(variableToDelay.getBooleanValue()); - } - this.set(variableToDelay.getBooleanValue()); - } - - void getInternalState(String inputString, Boolean ifDebug) - { - if (!ifDebug) - return; - - String string = inputString + "\nvalue = " + this.getBooleanValue() + "\n"; - for (int i = 0; i < previousYoVariables.length; i++) - { - string = string + i + " = " + previousYoVariables[i].getBooleanValue() + "\n"; - } - string = string + "variableToDelay = " + variableToDelay.getBooleanValue() + "\n"; - System.out.println(string); - } -} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java deleted file mode 100644 index 55da3af5..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DelayedYoDouble.java +++ /dev/null @@ -1,55 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; - -public class DelayedYoDouble extends YoDouble -{ - private final DoubleProvider variableToDelay; - - private final YoDouble[] previousYoVariables; - - public DelayedYoDouble(String name, String description, DoubleProvider variableToDelay, int ticksToDelay, YoRegistry registry) - { - super(name, description, registry); - - this.variableToDelay = variableToDelay; - previousYoVariables = new YoDouble[ticksToDelay]; - - for (int i = 0; i < ticksToDelay; i++) - { - previousYoVariables[i] = new YoDouble(name + "_previous" + i, registry); - previousYoVariables[i].set(variableToDelay.getValue()); - } - - this.set(variableToDelay.getValue()); - } - - public void update() - { - if (previousYoVariables.length == 0) - { - this.set(variableToDelay.getValue()); - return; - } - - this.set(previousYoVariables[0].getValue()); - - for (int i = 0; i < previousYoVariables.length - 1; i++) - { - previousYoVariables[i].set(previousYoVariables[i + 1].getValue()); - } - - previousYoVariables[previousYoVariables.length - 1].set(variableToDelay.getValue()); - } - - public void reset() - { - for (YoDouble var : previousYoVariables) - { - var.set(variableToDelay.getValue()); - } - this.set(variableToDelay.getValue()); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java deleted file mode 100644 index 5bd56a4c..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/DeltaLimitedYoVariable.java +++ /dev/null @@ -1,69 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -/** - * - * @author Doug Stephen (dstephen@ihmc.us) - */ -public class DeltaLimitedYoVariable extends YoDouble -{ - private final YoDouble maxDelta; - private final YoDouble actual; - private final YoDouble desired; - private final YoBoolean isLimitingActive; - - public DeltaLimitedYoVariable(String name, YoRegistry registry, double maxDelta) - { - super(name, registry); - - this.maxDelta = new YoDouble(name + "MaxAllowedDelta", registry); - this.maxDelta.set(Math.abs(maxDelta)); - this.actual = new YoDouble(name + "Actual", registry); - this.desired = new YoDouble(name + "Desired", registry); - this.isLimitingActive = new YoBoolean(name + "IsLimitingActive", registry); - isLimitingActive.set(false); - } - - public void setMaxDelta(double maxDelta) - { - this.maxDelta.set(Math.abs(maxDelta)); - } - - public void updateOutput(double actual, double desired) - { - this.desired.set(desired); - this.actual.set(actual); - updateOutput(); - } - - public boolean isLimitingActive() - { - return isLimitingActive.getBooleanValue(); - } - - private void updateOutput() - { - double actualDoubleValue = actual.getDoubleValue(); - double desiredDoubleValue = desired.getDoubleValue(); - double maxDeltaDoubleValue = Math.abs(maxDelta.getDoubleValue()); - double rawDelta = actualDoubleValue - desiredDoubleValue; - double sign = Math.signum(rawDelta); - double requestedDelta = Math.abs(rawDelta); - double overshoot = maxDeltaDoubleValue - requestedDelta; - - if(overshoot < 0) - { - desiredDoubleValue = actualDoubleValue - maxDeltaDoubleValue * sign; - isLimitingActive.set(true); - } - else - { - isLimitingActive.set(false); - } - - this.set(desiredDoubleValue); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java deleted file mode 100644 index 6a0ba403..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredDiscreteVelocityYoVariable.java +++ /dev/null @@ -1,201 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoEnum; - -/** - * @author jrebula - *

- * A YoFilteredVelocityVariable is a filtered velocity of a position. - * Either a YoVariable holding the position is passed in to the - * constructor and update() is called every tick, or update(double) is - * called every tick. The YoFilteredVelocityVariable updates it's val - * with the current velocity after a filter of - *

- * - * 
- *            vel_{n} = alpha * vel{n-1} + (1 - alpha) * (pos_{n} - pos_{n-1})
- *
- * - */ -public class FilteredDiscreteVelocityYoVariable extends YoDouble -{ - private final double alpha; - - private final YoDouble time; - - private final YoDouble alphaVariable; - private final YoDouble position; - - private final YoDouble lastUpdateTime; - private final YoEnum lastUpdateDirection; - private final YoDouble unfilteredVelocity; - - private final YoDouble lastPosition; - private boolean hasBeenCalled; - - public FilteredDiscreteVelocityYoVariable(String name, String description, double alpha, YoDouble time, YoRegistry registry) - { - super(name, description, registry); - - this.alpha = alpha; - this.alphaVariable = null; - this.position = null; - - this.time = time; - - lastPosition = new YoDouble(name + "_lastPosition", registry); - lastUpdateTime = new YoDouble(name + "_lastUpdateTime", registry); - lastUpdateDirection = new YoEnum<>(name + "_lastUpdateDirection", registry, Direction.class); - unfilteredVelocity = new YoDouble(name + "_unfilteredVelocity", registry); - - reset(); - } - - public FilteredDiscreteVelocityYoVariable(String name, String description, double alpha, YoDouble positionVariable, YoDouble time, - YoRegistry registry) - { - super(name, description, registry); - - this.alpha = alpha; - this.position = positionVariable; - - this.alphaVariable = null; - - this.time = time; - - lastPosition = new YoDouble(name + "_lastPosition", registry); - lastUpdateTime = new YoDouble(name + "_lastUpdateTime", registry); - lastUpdateDirection = new YoEnum<>(name + "_lastUpdateDirection", registry, Direction.class); - unfilteredVelocity = new YoDouble(name + "_unfilteredVelocity", registry); - - reset(); - } - - public FilteredDiscreteVelocityYoVariable(String name, String description, YoDouble alphaVariable, YoDouble positionVariable, - YoDouble time, YoRegistry registry) - { - super(name, description, registry); - position = positionVariable; - this.alphaVariable = alphaVariable; - this.alpha = 0.0; - - this.time = time; - - lastPosition = new YoDouble(name + "_lastPosition", registry); - lastUpdateTime = new YoDouble(name + "_lastUpdateTime", registry); - lastUpdateDirection = new YoEnum<>(name + "_lastUpdateDirection", registry, Direction.class); - unfilteredVelocity = new YoDouble(name + "_unfilteredVelocity", registry); - - reset(); - } - - public void reset() - { - hasBeenCalled = false; - } - - public void update() - { - if (position == null) - { - throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(position.getDoubleValue()); - } - - public void update(double currentPosition) - { - if (!hasBeenCalled) - { - hasBeenCalled = true; - - // lastPosition = currentPosition; - lastPosition.set(currentPosition); - lastUpdateTime.set(time.getDoubleValue()); - lastUpdateDirection.set(Direction.NONE); - } - - double alphaToUse = alpha; - - if (alphaVariable != null) - { - alphaToUse = alphaVariable.getDoubleValue(); - } - - // Figure out if the count changed and if so, if the direction changed or not and then update the direction: - boolean countChanged = false; - if (currentPosition != lastPosition.getDoubleValue()) - countChanged = true; - - // If the count changed, figure out if the direction changed: - - boolean directionChanged = false; - if (countChanged) - { - if (currentPosition > lastPosition.getDoubleValue()) - { - if (lastUpdateDirection.getEnumValue() != Direction.FORWARD) - directionChanged = true; - lastUpdateDirection.set(Direction.FORWARD); - } - else if (currentPosition < lastPosition.getDoubleValue()) - { - if (lastUpdateDirection.getEnumValue() != Direction.BACKWARD) - directionChanged = true; - lastUpdateDirection.set(Direction.BACKWARD); - } - } - - // If the direction changed, then the velocity is set to zero: - if (directionChanged) - { - unfilteredVelocity.set(0.0); - } - - // If the direction hasn't changed, but the count changed then compute the velocity based on the time since last update: - else if (countChanged) - { - double diffTime = time.getDoubleValue() - lastUpdateTime.getDoubleValue(); - if (diffTime < 1e-7) - unfilteredVelocity.set(0.0); - else - { - unfilteredVelocity.set((currentPosition - lastPosition.getDoubleValue()) / diffTime); - } - } - - else - { - // If the count hasn't changed, then not quite sure what the velocity is. - // We could just use the current velocity, but really should try to figure out if things have been slowing down or not. - // For now, multiply by some largish fraction, just to make sure it does trail off to zero if the velocity stops quickly. - unfilteredVelocity.set(0.99 * unfilteredVelocity.getDoubleValue()); - } - - // Low pass alpha filter it... - set(alphaToUse * getDoubleValue() + (1.0 - alphaToUse) * unfilteredVelocity.getDoubleValue()); - - // Remember the position and the currentTime if the countChanged: - - if (countChanged) - { - lastPosition.set(currentPosition); - lastUpdateTime.set(time.getDoubleValue()); - } - } - - public double getUnfilteredVelocity() - { - return unfilteredVelocity.getDoubleValue(); - } - - private enum Direction - { - NONE, FORWARD, BACKWARD; - } - -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java deleted file mode 100644 index 334fcd9e..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector.java +++ /dev/null @@ -1,134 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; - -/** - *

- * FilteredVelocityYoFrameVector - *

- * - *

- * Differentiates and Filters a YoFrameVector to get its derivative. - *

- * - *

- * IHMC - *

- * - * 
- *            vel_{n} = alpha * vel{n-1} + (1 - alpha) * (pos_{n} - pos_{n-1})
- *
- * - * @author IHMC Biped Team - * @version 1.0 - */ -public class FilteredVelocityYoFrameVector extends YoFrameVector3D -{ - private final double dt; - private final DoubleProvider alphaProvider; - - private final YoBoolean hasBeenCalled; - private final FrameTuple3DReadOnly currentPosition; - private final YoFrameVector3D lastPosition; - - /** - * @deprecated Use - * {@link #FilteredVelocityYoFrameVector(String, String, DoubleProvider, double, YoRegistry, FrameTuple3DReadOnly)} - * instead. - */ - @Deprecated - public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, - YoRegistry registry, - FrameTuple3DReadOnly frameTuple3DToDifferentiate) - { - return new FilteredVelocityYoFrameVector(namePrefix, nameSuffix, alpha, dt, registry, frameTuple3DToDifferentiate); - } - - /** - * @deprecated Use - * {@link #FilteredVelocityYoFrameVector(String, String, DoubleProvider, double, YoRegistry, ReferenceFrame)} - * instead. - */ - @Deprecated - public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, - YoRegistry registry, ReferenceFrame referenceFrame) - { - return new FilteredVelocityYoFrameVector(namePrefix, nameSuffix, alpha, dt, registry, referenceFrame); - } - - public FilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoRegistry registry, - FrameTuple3DReadOnly frameTuple3DToDifferentiate) - { - super(namePrefix, nameSuffix, frameTuple3DToDifferentiate.getReferenceFrame(), registry); - this.alphaProvider = alpha; - this.dt = dt; - - hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); - currentPosition = frameTuple3DToDifferentiate; - lastPosition = new YoFrameVector3D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); - reset(); - } - - public FilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoRegistry registry, - ReferenceFrame referenceFrame) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - this.alphaProvider = alpha; - this.dt = dt; - - hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); - currentPosition = null; - lastPosition = new YoFrameVector3D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (currentPosition == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(FrameTuple3DReadOnly)"); - } - - update(currentPosition); - } - - public void update(FrameTuple3DReadOnly frameTuple) - { - checkReferenceFrameMatch(frameTuple); - update((Tuple3DReadOnly) frameTuple); - } - - private final Vector3D currentRawDerivative = new Vector3D(); - - public void update(Tuple3DReadOnly currentPosition) - { - if (!hasBeenCalled.getBooleanValue()) - { - hasBeenCalled.set(true); - lastPosition.set(currentPosition); - setToZero(); - } - - currentRawDerivative.sub(currentPosition, lastPosition); - currentRawDerivative.scale(1.0 / dt); - - double alpha = alphaProvider.getValue(); - interpolate(currentRawDerivative, this, alpha); - - lastPosition.set(currentPosition); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java deleted file mode 100644 index ab519961..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoFrameVector2d.java +++ /dev/null @@ -1,126 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; -import us.ihmc.euclid.tuple2D.Vector2D; -import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; - -public class FilteredVelocityYoFrameVector2d extends YoFrameVector2D -{ - private final double dt; - private final DoubleProvider alphaProvider; - - private final YoBoolean hasBeenCalled; - private final FrameTuple2DReadOnly currentPosition; - private final YoFrameVector2D lastPosition; - - /** - * @deprecated Use - * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoRegistry, FrameTuple2DReadOnly)} - * instead. - */ - @Deprecated - public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, - YoRegistry registry, - FrameTuple2DReadOnly frameTuple2DToDifferentiate) - { - return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, frameTuple2DToDifferentiate); - } - - /** - * @deprecated Use - * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoRegistry, FrameTuple2DReadOnly)} - * instead. - */ - @Deprecated - public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, String description, - DoubleProvider alpha, double dt, YoRegistry registry, - FrameTuple2DReadOnly frameTuple2DToDifferentiate) - { - return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, frameTuple2DToDifferentiate); - } - - /** - * @deprecated Use - * {@link #FilteredVelocityYoFrameVector2d(String, String, DoubleProvider, double, YoRegistry, ReferenceFrame)} - * instead. - */ - @Deprecated - public static FilteredVelocityYoFrameVector2d createFilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, - YoRegistry registry, ReferenceFrame referenceFrame) - { - return new FilteredVelocityYoFrameVector2d(namePrefix, nameSuffix, alpha, dt, registry, referenceFrame); - } - - public FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoRegistry registry, - FrameTuple2DReadOnly frameTuple2DToDifferentiate) - { - super(namePrefix, nameSuffix, frameTuple2DToDifferentiate.getReferenceFrame(), registry); - this.alphaProvider = alpha; - this.dt = dt; - - hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); - currentPosition = frameTuple2DToDifferentiate; - lastPosition = new YoFrameVector2D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); - reset(); - } - - public FilteredVelocityYoFrameVector2d(String namePrefix, String nameSuffix, DoubleProvider alpha, double dt, YoRegistry registry, - ReferenceFrame referenceFrame) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - this.alphaProvider = alpha; - this.dt = dt; - - hasBeenCalled = new YoBoolean(namePrefix + nameSuffix + "HasBeenCalled", registry); - currentPosition = null; - lastPosition = new YoFrameVector2D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (currentPosition == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(FrameTuple3DReadOnly)"); - } - - update(currentPosition); - } - - public void update(FrameTuple2DReadOnly frameTuple) - { - checkReferenceFrameMatch(frameTuple); - update((Tuple2DReadOnly) frameTuple); - } - - private final Vector2D currentRawDerivative = new Vector2D(); - - public void update(Tuple2DReadOnly currentPosition) - { - if (!hasBeenCalled.getBooleanValue()) - { - hasBeenCalled.set(true); - lastPosition.set(currentPosition); - setToZero(); - } - - currentRawDerivative.sub(currentPosition, lastPosition); - currentRawDerivative.scale(1.0 / dt); - - interpolate(currentRawDerivative, this, alphaProvider.getValue()); - - lastPosition.set(currentPosition); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java deleted file mode 100644 index f8bd502c..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FilteredVelocityYoVariable.java +++ /dev/null @@ -1,182 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.robotics.geometry.AngleTools; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -/** - * @author jrebula - *

- * A YoFilteredVelocityVariable is a filtered velocity of a position. - * Either a YoVariable holding the position is passed in to the - * constructor and update() is called every tick, or update(double) is - * called every tick. The YoFilteredVelocityVariable updates it's val - * with the current velocity after a filter of - *

- * - * 
- *            vel_{n} = alpha * vel{n-1} + (1 - alpha) * (pos_{n} - pos_{n-1})
- *
- * - */ -public class FilteredVelocityYoVariable extends YoDouble implements ProcessingYoVariable -{ - private double alphaDouble; - private final double dt; - - private final DoubleProvider alphaVariable; - private final YoDouble position; - -// private double lastPosition; - private final YoDouble lastPosition; - private final YoBoolean hasBeenCalled; - - public FilteredVelocityYoVariable(String name, String description, double alpha, double dt, YoRegistry registry) - { - super(name, description, registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - - this.alphaDouble = alpha; - this.dt = dt; - - this.alphaVariable = null; - this.position = null; - - lastPosition = new YoDouble(name + "_lastPosition", registry); - - reset(); - } - - public FilteredVelocityYoVariable(String name, String description, double alpha, YoDouble positionVariable, double dt, YoRegistry registry) - { - super(name, description, registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - - this.alphaDouble = alpha; - this.position = positionVariable; - this.dt = dt; - - this.alphaVariable = null; - - lastPosition = new YoDouble(name + "_lastPosition", registry); - - - reset(); - } - - public FilteredVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, YoDouble positionVariable, double dt, YoRegistry registry) - { - super(name, description, registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - - position = positionVariable; - this.alphaVariable = alphaVariable; - this.alphaDouble = 0.0; - - this.dt = dt; - - lastPosition = new YoDouble(name + "_lastPosition", registry); - - reset(); - } - - public FilteredVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, double dt, YoRegistry registry) - { - super(name, description, registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - - this.position = null; - this.alphaVariable = alphaVariable; - this.alphaDouble = 0.0; - - this.dt = dt; - - lastPosition = new YoDouble(name + "_lastPosition", registry); - - reset(); - } - - @Override - public void reset() - { - hasBeenCalled.set(false); - } - - @Override - public void update() - { - if (position == null) - { - throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(position.getDoubleValue()); - } - - public void updateForAngles() - { - if (position == null) - { - throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - updateForAngles(position.getDoubleValue()); - } - - public void update(double currentPosition) - { - if (!hasBeenCalled.getBooleanValue()) - { - hasBeenCalled.set(true); - lastPosition.set(currentPosition); - set(0.0); - } - - double difference = currentPosition - lastPosition.getDoubleValue(); - - updateUsingDifference(difference); - - lastPosition.set(currentPosition); - } - - public void updateForAngles(double currentPosition) - { - if (!hasBeenCalled.getBooleanValue()) - { - hasBeenCalled.set(true); - lastPosition.set(currentPosition); - set(0.0); - } - - double difference = AngleTools.computeAngleDifferenceMinusPiToPi(currentPosition, lastPosition.getDoubleValue()); - - updateUsingDifference(difference); - - lastPosition.set(currentPosition); - } - - private void updateUsingDifference(double difference) - { - double previousFilteredDerivative = getDoubleValue(); - double currentRawDerivative = difference / dt; - - double alpha = alphaVariable == null ? alphaDouble : alphaVariable.getValue(); - set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); - } - - public void setAlpha(double alpha) - { - if (alphaVariable == null) - { - this.alphaDouble = alpha; - } - else - { - throw new RuntimeException("A double provider was used to construct this filtered variable. Modyfy the value of that provider directly."); - } - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java deleted file mode 100644 index 0f97d89a..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FiniteDifferenceAngularVelocityYoFrameVector.java +++ /dev/null @@ -1,97 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.axisAngle.AxisAngle; -import us.ihmc.euclid.matrix.RotationMatrix; -import us.ihmc.euclid.matrix.interfaces.RotationMatrixReadOnly; -import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; - -public class FiniteDifferenceAngularVelocityYoFrameVector extends YoFrameVector3D -{ - private final YoFrameQuaternion orientation; - private final YoFrameQuaternion orientationPreviousValue; - - private final YoBoolean hasBeenCalled; - - private final RotationMatrix currentOrientationMatrix = new RotationMatrix(); - private final RotationMatrix previousOrientationMatrix = new RotationMatrix(); - private final RotationMatrix deltaOrientationMatrix = new RotationMatrix(); - private final AxisAngle deltaAxisAngle = new AxisAngle(); - - private final double dt; - - public FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, ReferenceFrame referenceFrame, double dt, YoRegistry registry) - { - this(namePrefix, null, referenceFrame, dt, registry); - } - - public FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, YoFrameQuaternion orientationToDifferentiate, double dt, YoRegistry registry) - { - this(namePrefix, orientationToDifferentiate, orientationToDifferentiate.getReferenceFrame(), dt, registry); - } - - private FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, YoFrameQuaternion orientationToDifferentiate, ReferenceFrame referenceFrame, double dt, YoRegistry registry) - { - super(namePrefix, referenceFrame, registry); - - this.dt = dt; - - orientation = orientationToDifferentiate; - orientationPreviousValue = new YoFrameQuaternion(namePrefix + "_previous", referenceFrame, registry); - - hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled", registry); - hasBeenCalled.set(false); - } - - public void update() - { - if (orientation == null) - { - throw new NullPointerException("FiniteDifferenceAngularVelocityYoFrameVector must be constructed with a non null " - + "orientation variable to call update(), otherwise use update(FrameOrientation)"); - } - - currentOrientationMatrix.set(orientation); - update(currentOrientationMatrix); - } - - public void update(FrameOrientation3DReadOnly currentOrientation) - { - checkReferenceFrameMatch(currentOrientation); - - currentOrientationMatrix.set(currentOrientation); - update(currentOrientationMatrix); - } - - public void update(Orientation3DReadOnly currentOrientation) - { - currentOrientationMatrix.set(currentOrientation); - update(currentOrientationMatrix); - } - - public void update(RotationMatrixReadOnly rotationMatrix) - { - if (!hasBeenCalled.getBooleanValue()) - { - orientationPreviousValue.set(rotationMatrix); - hasBeenCalled.set(true); - } - - if (rotationMatrix != currentOrientationMatrix) - currentOrientationMatrix.set(rotationMatrix); - previousOrientationMatrix.set(orientationPreviousValue); - deltaOrientationMatrix.set(currentOrientationMatrix); - deltaOrientationMatrix.multiplyTransposeOther(previousOrientationMatrix); - deltaAxisAngle.set(deltaOrientationMatrix); - - set(deltaAxisAngle.getX(), deltaAxisAngle.getY(), deltaAxisAngle.getZ()); - scale(deltaAxisAngle.getAngle() / dt); - - orientationPreviousValue.set(currentOrientationMatrix); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java deleted file mode 100644 index ee9be329..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderBandPassFilteredYoVariable.java +++ /dev/null @@ -1,68 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; - -public class FirstOrderBandPassFilteredYoVariable extends FirstOrderFilteredYoVariable -{ - private boolean hasBeenCalled = false; - - private final FirstOrderFilteredYoVariable highPassFilteredInput; - - public FirstOrderBandPassFilteredYoVariable(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz, - YoDouble yoTime, YoRegistry registry) - { - super(name, description, maxPassThroughFrequency_Hz, yoTime, FirstOrderFilterType.LOW_PASS, registry); - - this.highPassFilteredInput = new FirstOrderFilteredYoVariable(name + "HighPassFilteredOnly", description, minPassThroughFrequency_Hz, yoTime, - FirstOrderFilterType.HIGH_PASS, registry); - - setPassBand(minPassThroughFrequency_Hz, maxPassThroughFrequency_Hz); - } - - public FirstOrderBandPassFilteredYoVariable(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz, - double DT, YoRegistry registry) - { - super(name, description, maxPassThroughFrequency_Hz, DT, FirstOrderFilterType.LOW_PASS, registry); - - this.highPassFilteredInput = new FirstOrderFilteredYoVariable(name + "HighPassFilteredOnly", description, minPassThroughFrequency_Hz, DT, - FirstOrderFilterType.HIGH_PASS, registry); - } - - private void checkPassband(double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz) - { - if (minPassThroughFrequency_Hz > maxPassThroughFrequency_Hz) - { - throw new RuntimeException("minPassThroughFrequency [ " + minPassThroughFrequency_Hz + " ] > maxPassThroughFrequency [ " + maxPassThroughFrequency_Hz - + " ]"); - } - } - - public void update(double filterInput) - { - if (!hasBeenCalled) - { - hasBeenCalled = true; - this.set(filterInput); - } - else - { - updateHighPassFilterAndThenLowPassFilterThat(filterInput); - } - } - - public void setPassBand(double minPassThroughFreqHz, double maxPassThroughFreqHz) - { - checkPassband(minPassThroughFreqHz, maxPassThroughFreqHz); - - highPassFilteredInput.setCutoffFrequencyHz(minPassThroughFreqHz); - this.setCutoffFrequencyHz(maxPassThroughFreqHz); - } - - private void updateHighPassFilterAndThenLowPassFilterThat(double filterInput) - { - this.highPassFilteredInput.update(filterInput); - - super.update(highPassFilteredInput.getDoubleValue()); - } -} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java deleted file mode 100644 index ab2caa2f..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/FirstOrderFilteredYoVariable.java +++ /dev/null @@ -1,157 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; - -public class FirstOrderFilteredYoVariable extends YoDouble - { - - public enum FirstOrderFilterType - { - LOW_PASS, HIGH_PASS - } - - private boolean hasBeenCalled = false; - - private double filterInputOld; - private double filterUpdateTimeOld; - - private final YoDouble cutoffFrequency_Hz; - - private final YoDouble yoTime; - private double dt; - - private FirstOrderFilterType highOrLowPass; - - public FirstOrderFilteredYoVariable(String name, String description, double cutoffFrequency_Hz, YoDouble yoTime, FirstOrderFilterType highOrLowPass, YoRegistry registry) - { - super(name, description, registry); - - String cutoffFrequencyName; - switch (highOrLowPass) - { - case LOW_PASS: - cutoffFrequencyName = name + "_LowPassCutoff_Hz"; - break; - case HIGH_PASS: - cutoffFrequencyName = name + "_HighPassCutoff_Hz"; - break; - default: - throw new RuntimeException("Must Specify Filter Type as Low or High Pass. Current Specification : " + highOrLowPass); - } - - this.cutoffFrequency_Hz = new YoDouble(cutoffFrequencyName, registry); - this.cutoffFrequency_Hz.set(cutoffFrequency_Hz); - - this.yoTime = yoTime; - - this.highOrLowPass = highOrLowPass; - } - - public FirstOrderFilteredYoVariable(String name, String description, double cutoffFrequency_Hz, double DT, FirstOrderFilterType highOrLowPass, YoRegistry registry) - { - this(name, description, cutoffFrequency_Hz, null, highOrLowPass, registry); - this.dt = DT; - } - - private double computeLowPassUpdate(double filterInput, double dt) - { - double alpha = computeAlpha( dt, cutoffFrequency_Hz.getDoubleValue()); - - double ret = alpha * this.getDoubleValue() + (1.0-alpha) * filterInput; - - return ret; - } - - private double computeHighPassUpdate(double filterInput, double dt) - { - double alpha = computeAlpha(dt, cutoffFrequency_Hz.getDoubleValue()); - - double ret = alpha * (this.getDoubleValue() + filterInput - filterInputOld); - return ret; - } - - private double computeAlpha(double dt, double cutoffFrequencyHz) - { - if (cutoffFrequencyHz <= 0.0) - { - throw new RuntimeException("Cutoff Frequency must be greater than zero. Cutoff = " + cutoffFrequencyHz); - } - - double cutoff_radPerSec = cutoffFrequencyHz * 2.0 * Math.PI; - double RC = 1.0 / cutoff_radPerSec; - double alpha = RC / (RC + dt); // alpha decreases with increasing cutoff frequency - - if (alpha <= 0 || alpha >= 1.0 && dt != 0.0) - { - throw new RuntimeException("Alpha value must be between 0 and 1. Alpha = " + alpha); - } - - return alpha; - } - - public void reset() - { - hasBeenCalled = false; - } - - public void setCutoffFrequencyHz(double cutoffHz) - { - this.cutoffFrequency_Hz.set(cutoffHz); - } - - public void update(double filterInput) - { - if (!hasBeenCalled) - { - hasBeenCalled = true; - - filterInputOld = 0.0; - filterUpdateTimeOld = 0.0; - - this.set(filterInput); - } - else - { - if ( yoTime != null ) - { - double timeSinceLastUpdate = yoTime.getDoubleValue() - filterUpdateTimeOld; - - if (timeSinceLastUpdate > 0.0) - { - dt = timeSinceLastUpdate; - } - else - { - reset(); -// throw new RuntimeException("Computed step size, DT must be greater than zero. DT = " + dt + ". Current time = " + yoTime.getDoubleValue() + ", previous update time = " + filterUpdateTimeOld); - } - } - - double filterOutput; - - switch (highOrLowPass) - { - case LOW_PASS: - filterOutput = computeLowPassUpdate(filterInput, dt); - break; - case HIGH_PASS: - filterOutput = computeHighPassUpdate(filterInput, dt); - break; - - default: - filterOutput = filterInput; - break; - } - - this.set(filterOutput); - } - - filterInputOld = filterInput; - - if (yoTime != null) - { - filterUpdateTimeOld = yoTime.getDoubleValue(); - } - } - } \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java deleted file mode 100644 index 530dab44..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoBoolean.java +++ /dev/null @@ -1,122 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoInteger; - -public class GlitchFilteredYoBoolean extends YoBoolean -{ - private YoBoolean variableToFilter; - private final YoInteger windowSize; - protected final YoInteger counter; - - public GlitchFilteredYoBoolean(String name, int windowSize) - { - super(name, "GlitchFilteredYoBoolean", null); - counter = new YoInteger(name + "Count", getRegistry()); - this.windowSize = new YoInteger(name + "WindowSize", getRegistry()); - - initialize(null, windowSize); - } - - public GlitchFilteredYoBoolean(String name, YoRegistry registry, int windowSize) - { - super(name, registry); - counter = new YoInteger(name + "Count", registry); - this.windowSize = new YoInteger(name + "WindowSize", registry); - - initialize(null, windowSize); - } - - public GlitchFilteredYoBoolean(String name, YoBoolean yoVariableToFilter, int windowSize) - { - super(name, "GlitchFilteredYoBoolean", null); - counter = new YoInteger(name + "Count", getRegistry()); - this.windowSize = new YoInteger(name + "WindowSize", getRegistry()); - - initialize(yoVariableToFilter, windowSize); - } - - public GlitchFilteredYoBoolean(String name, YoRegistry registry, YoBoolean yoVariableToFilter, int windowSize) - { - this(name, "", registry, yoVariableToFilter, windowSize); - } - - public GlitchFilteredYoBoolean(String name, YoRegistry registry, YoBoolean yoVariableToFilter, YoInteger windowSize) - { - this(name, "", registry, yoVariableToFilter, windowSize); - } - - public GlitchFilteredYoBoolean(String name, String description, YoRegistry registry, YoBoolean yoVariableToFilter, int windowSize) - { - this(name, description, registry, yoVariableToFilter, new YoInteger(name + "WindowSize", description, registry)); - - this.windowSize.set(windowSize); - } - - public GlitchFilteredYoBoolean(String name, String description, YoRegistry registry, YoBoolean yoVariableToFilter, YoInteger windowSize) - { - super(name, description, registry); - counter = new YoInteger(name + "Count", description, registry); - this.windowSize = windowSize; - - initialize(yoVariableToFilter, windowSize.getIntegerValue()); - } - - private void initialize(YoBoolean yoVariableToFilter, int windowSize) - { - if (windowSize < 0) - throw new RuntimeException("window size must be greater than 0"); - - variableToFilter = yoVariableToFilter; - this.windowSize.set(windowSize); - - if (variableToFilter != null) - this.set(yoVariableToFilter.getBooleanValue()); - - this.set(false); - } - - public boolean set(boolean value) - { - if (counter != null) - { - counter.set(0); - } - return super.set(value); - } - - public void update(boolean value) - { - - if (value != this.getBooleanValue()) - { - counter.set(counter.getIntegerValue() + 1); - } - else - counter.set(0); - - if (counter.getIntegerValue() >= (windowSize.getIntegerValue())) - set(value); - } - - public int getWindowSize() - { - return windowSize.getIntegerValue(); - } - - public void setWindowSize(int windowSize) //untested - { - this.windowSize.set(windowSize); - } - - public void update() - { - if (variableToFilter == null) - { - throw new RuntimeException("variableToFilter was not initialized. Use the other constructor"); - } - else - update(variableToFilter.getBooleanValue()); - } -} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java deleted file mode 100644 index 4ee14989..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/GlitchFilteredYoInteger.java +++ /dev/null @@ -1,107 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.providers.IntegerProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoInteger; - -public class GlitchFilteredYoInteger extends YoInteger -{ - private final YoInteger position; - private final YoInteger previousPosition; - private final IntegerProvider windowSize; - private final YoInteger counter; - - public GlitchFilteredYoInteger(String name, int windowSize, YoRegistry registry) - { - this(name, windowSize, null, registry); - } - - public GlitchFilteredYoInteger(String name, int windowSize, YoInteger position, YoRegistry registry) - { - super(name, GlitchFilteredYoInteger.class.getSimpleName(), registry); - - this.position = position; - - previousPosition = new YoInteger(name + "PrevValue", registry); - counter = new YoInteger(name + "Count", registry); - YoInteger yoWindowSize = new YoInteger(name + "WindowSize", registry); - yoWindowSize.set(windowSize); - this.windowSize = yoWindowSize; - } - - public GlitchFilteredYoInteger(String name, IntegerProvider windowSize, YoRegistry registry) - { - this(name, windowSize, null, registry); - } - - public GlitchFilteredYoInteger(String name, IntegerProvider windowSize, YoInteger position, YoRegistry registry) - { - super(name, GlitchFilteredYoInteger.class.getSimpleName(), registry); - - this.position = position; - - previousPosition = new YoInteger(name + "PrevValue", registry); - counter = new YoInteger(name + "Count", registry); - this.windowSize = windowSize; - } - - @Override - public boolean set(int value) - { - if (counter != null) - counter.set(0); - return super.set(value); - } - - @Override - public boolean set(int value, boolean notifyListeners) - { - if (counter != null) - counter.set(0); - return super.set(value, notifyListeners); - } - - public void update() - { - if (position == null) - { - throw new NullPointerException( - "GlitchFilteredYoInteger must be constructed with a non null position variable to call update(), otherwise use update(int)"); - } - - update(position.getIntegerValue()); - } - - public void update(int currentValue) - { - if (currentValue == previousPosition.getIntegerValue()) - counter.increment(); - else - counter.set(0); - - if (counter.getIntegerValue() >= windowSize.getValue()) - { - set(currentValue); - counter.set(0); - } - - previousPosition.set(currentValue); - } - - public int getWindowSize() - { - return windowSize.getValue(); - } - - public void setWindowSize(int windowSize) - { - if (this.windowSize instanceof YoInteger) - { - ((YoInteger) this.windowSize).set(windowSize); - } - else - { - throw new RuntimeException("Setting the window size is not supported"); - } - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java deleted file mode 100644 index 05a6c61e..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/JerkLimitedYoVariable.java +++ /dev/null @@ -1,151 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.commons.MathTools; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class JerkLimitedYoVariable extends YoDouble -{ - private final double dt; - - private final YoBoolean hasBeenInitialized; - - private final YoDouble smoothedRate; - private final YoDouble smoothedAcceleration; - private final YoDouble smoothedJerk; - - private final YoDouble positionGain; - private final YoDouble velocityGain; - private final YoDouble accelerationGain; - - private final YoDouble maximumJerk; - private final YoDouble maximumAcceleration; - - private final YoDouble inputPosition; - private final YoDouble inputVelocity; - private final YoDouble inputAcceleration; - - public JerkLimitedYoVariable(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, double dt) - { - this(name, registry, maxAcceleration, maxJerk, null, null, null, dt); - } - - public JerkLimitedYoVariable(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, - YoDouble inputPosition, double dt) - { - this(name, registry, maxAcceleration, maxJerk, inputPosition, null, null, dt); - } - - public JerkLimitedYoVariable(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, - YoDouble inputPosition, YoDouble inputVelocity, double dt) - { - this(name, registry, maxAcceleration, maxJerk, inputPosition, inputVelocity, null, dt); - } - - public JerkLimitedYoVariable(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, - YoDouble inputPosition, YoDouble inputVelocity, YoDouble inputAcceleration, double dt) - { - super(name, registry); - - this.inputPosition = inputPosition; - this.inputVelocity = inputVelocity; - this.inputAcceleration = inputAcceleration; - - this.maximumJerk = maxJerk; - this.maximumAcceleration = maxAcceleration; - - this.dt = dt; - - hasBeenInitialized = new YoBoolean(name + "HasBeenInitialized", registry); - - smoothedRate = new YoDouble(name + "SmoothedRate", registry); - smoothedAcceleration = new YoDouble(name + "SmoothedAcceleration", registry); - smoothedJerk = new YoDouble(name + "SmoothedJerk", registry); - - positionGain = new YoDouble(name + "PositionGain", registry); - velocityGain = new YoDouble(name + "VelocityGain", registry); - accelerationGain = new YoDouble(name + "AccelerationGain", registry); - - double w0 = 2.0 * Math.PI * 16.0; - double w1 = 2.0 * Math.PI * 16.0; - double zeta = 1.0; - - setGainsByPolePlacement(w0, w1, zeta); - hasBeenInitialized.set(false); - } - - public void setMaximumAcceleration(double maximumAcceleration) - { - this.maximumAcceleration.set(maximumAcceleration); - } - - public void setMaximumJerk(double maximumJerk) - { - this.maximumJerk.set(maximumJerk); - } - - public void setGainsByPolePlacement(double w0, double w1, double zeta) - { - positionGain.set(w0 * w1 * w1); - velocityGain.set(w1 * w1 + 2.0 * zeta * w1 * w0); - accelerationGain.set(w0 + 2.0 * zeta * w1); - } - - public void update() - { - double inputPosition = this.inputPosition == null ? 0.0 : this.inputPosition.getDoubleValue(); - double inputVelocity = this.inputVelocity == null ? 0.0 : this.inputVelocity.getDoubleValue(); - double inputAcceleration = this.inputAcceleration == null ? 0.0 : this.inputAcceleration.getDoubleValue(); - - update(inputPosition, inputVelocity, inputAcceleration); - } - - public void update(double inputPosition) - { - update(inputPosition, 0.0, 0.0); - } - - public void update(double inputPosition, double inputVelocity) - { - update(inputPosition, inputVelocity, 0.0); - } - - public void update(double inputPosition, double inputVelocity, double inputAcceleration) - { - if (!hasBeenInitialized.getBooleanValue()) - initialize(inputPosition, inputVelocity, inputAcceleration); - - double positionError = inputPosition - this.getDoubleValue(); - double velocityError = inputVelocity - smoothedRate.getDoubleValue(); - double accelerationError = inputAcceleration - smoothedAcceleration.getDoubleValue(); - double jerk = accelerationGain.getDoubleValue() * accelerationError + velocityGain.getDoubleValue() * velocityError + positionGain.getDoubleValue() - * positionError; - jerk = MathTools.clamp(jerk, -maximumJerk.getDoubleValue(), maximumJerk.getDoubleValue()); - - smoothedJerk.set(jerk); - smoothedAcceleration.add(smoothedJerk.getDoubleValue() * dt); - smoothedAcceleration.set(MathTools.clamp(smoothedAcceleration.getDoubleValue(), maximumJerk.getDoubleValue())); - smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); - this.add(smoothedRate.getDoubleValue() * dt); - } - - public void initialize(double inputPosition, double inputVelocity, double inputAcceleration) - { - this.set(inputPosition); - smoothedRate.set(inputVelocity); - smoothedAcceleration.set(inputAcceleration); - smoothedJerk.set(0.0); - - this.hasBeenInitialized.set(true); - } - - public void reset() - { - this.hasBeenInitialized.set(false); - smoothedRate.set(0.0); - smoothedAcceleration.set(0.0); - smoothedJerk.set(0.0); - } - -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java deleted file mode 100644 index 14b60182..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ProcessingYoVariable.java +++ /dev/null @@ -1,10 +0,0 @@ -package us.ihmc.robotics.math.filters; - -public interface ProcessingYoVariable -{ - public abstract void update(); - - public default void reset() - { - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java deleted file mode 100644 index 1c7ec5fb..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameOrientation.java +++ /dev/null @@ -1,152 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.FrameQuaternion; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameYawPitchRoll; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class RateLimitedYoFrameOrientation extends YoFrameYawPitchRoll -{ - private final DoubleProvider maxRateVariable; - - private final YoFrameYawPitchRoll rawOrientation; - private final YoBoolean limited; - private final YoBoolean hasBeenCalled; - private final double dt; - - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); - maxRate.set(initialValue); - return maxRate; - } - - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - YoFrameYawPitchRoll rawOrientation) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, rawOrientation, rawOrientation.getReferenceFrame()); - } - - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); - } - - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, - YoFrameYawPitchRoll rawOrientation) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawOrientation, - rawOrientation.getReferenceFrame()); - } - - public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, - ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); - } - - private RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - YoFrameYawPitchRoll rawOrientation, ReferenceFrame referenceFrame) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); - this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); - - if (maxRate == null) - maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); - - maxRateVariable = maxRate; - - this.rawOrientation = rawOrientation; - - this.dt = dt; - - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (rawOrientation == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(rawOrientation); - } - - public void update(YoFrameYawPitchRoll yoFrameVectorUnfiltered) - { - checkReferenceFrameMatch(yoFrameVectorUnfiltered); - update(yoFrameVectorUnfiltered.getYaw(), yoFrameVectorUnfiltered.getPitch(), yoFrameVectorUnfiltered.getRoll()); - } - - public void update(FrameQuaternion frameOrientationUnfiltered) - { - checkReferenceFrameMatch(frameOrientationUnfiltered); - update((QuaternionReadOnly) frameOrientationUnfiltered); - } - - private final Quaternion quaternionUnfiltered = new Quaternion(); - - public void update(double yawUnfiltered, double pitchUnfiltered, double rollUnfiltered) - { - quaternionUnfiltered.setYawPitchRoll(yawUnfiltered, pitchUnfiltered, rollUnfiltered); - update(quaternionUnfiltered); - } - - private final Quaternion quaternionFiltered = new Quaternion(); - private final Quaternion difference = new Quaternion(); - private final Vector3D limitedRotationVector = new Vector3D(); - - public void update(QuaternionReadOnly quaternionUnfiltered) - { - if (!hasBeenCalled.getBooleanValue() || containsNaN()) - { - hasBeenCalled.set(true); - limited.set(false); - set(quaternionUnfiltered); - return; - } - - quaternionFiltered.set(this); - - if (quaternionFiltered.dot(quaternionUnfiltered) > 0.0) - { - difference.difference(quaternionFiltered, quaternionUnfiltered); - } - else - { - difference.setAndNegate(quaternionUnfiltered); - difference.preMultiplyConjugateOther(quaternionFiltered); - } - - difference.getRotationVector(limitedRotationVector); - boolean clipped = limitedRotationVector.clipToMaxLength(dt * maxRateVariable.getValue()); - limited.set(clipped); - - if (clipped) - { - difference.setRotationVector(limitedRotationVector); - quaternionFiltered.multiply(difference); - set(quaternionFiltered); - } - else - { - set(quaternionUnfiltered); - } - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java deleted file mode 100644 index cea995da..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint.java +++ /dev/null @@ -1,120 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.FrameVector3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class RateLimitedYoFramePoint extends YoFramePoint3D -{ - private final DoubleProvider maxRateVariable; - - private final FrameTuple3DReadOnly rawPosition; - private final YoBoolean limited; - private final YoBoolean hasBeenCalled; - private final double dt; - - private final FrameVector3D differenceVector = new FrameVector3D(); - - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); - maxRate.set(initialValue); - return maxRate; - } - - public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - FrameTuple3DReadOnly rawPosition) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); - } - - public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); - } - - public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, - FrameTuple3DReadOnly rawPosition) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, - rawPosition.getReferenceFrame()); - } - - public RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); - } - - private RateLimitedYoFramePoint(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); - this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); - - if (maxRate == null) - maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); - - maxRateVariable = maxRate; - - this.rawPosition = rawPosition; - - this.dt = dt; - - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (rawPosition == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(rawPosition); - } - - public void update(FrameTuple3DReadOnly frameVectorUnfiltered) - { - checkReferenceFrameMatch(frameVectorUnfiltered); - update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); - } - - public void update(Tuple3DReadOnly vectorUnfiltered) - { - update(vectorUnfiltered.getX(), vectorUnfiltered.getY(), vectorUnfiltered.getZ()); - } - - public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) - { - if (!hasBeenCalled.getBooleanValue() || containsNaN()) - { - hasBeenCalled.set(true); - set(xUnfiltered, yUnfiltered, zUnfiltered); - } - - if (maxRateVariable.getValue() < 0) - throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); - - differenceVector.setToZero(getReferenceFrame()); - differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); - differenceVector.sub(getX(), getY(), getZ()); - - limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); - add(differenceVector); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java deleted file mode 100644 index b138bf53..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePoint2D.java +++ /dev/null @@ -1,134 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.FrameVector2D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class RateLimitedYoFramePoint2D extends YoFramePoint2D -{ - private final DoubleProvider maxRateVariable; - - private final FrameTuple2DReadOnly rawPosition; - private final YoBoolean limited; - private final YoBoolean hasBeenCalled; - private final double dt; - - private final FrameVector2D differenceVector = new FrameVector2D(); - - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); - maxRate.set(initialValue); - return maxRate; - } - - public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - FrameTuple2DReadOnly rawPosition) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); - } - - public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); - } - - public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, - FrameTuple2DReadOnly rawPosition) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, - rawPosition.getReferenceFrame()); - } - - public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); - } - - private RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - FrameTuple2DReadOnly rawPosition, ReferenceFrame referenceFrame) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); - this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); - - if (maxRate == null) - maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); - - maxRateVariable = maxRate; - - this.rawPosition = rawPosition; - - this.dt = dt; - - reset(); - } - - public void setAndUpdate(FramePoint2DReadOnly framePoint2D) - { - super.set(framePoint2D); - hasBeenCalled.set(true); - } - - public void setAndUpdate(FramePoint3DReadOnly framePoint3D) - { - super.set(framePoint3D); - hasBeenCalled.set(true); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (rawPosition == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(rawPosition); - } - - public void update(FrameTuple2DReadOnly frameVectorUnfiltered) - { - checkReferenceFrameMatch(frameVectorUnfiltered); - update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY()); - } - - public void update(Tuple2DReadOnly vectorUnfiltered) - { - update(vectorUnfiltered.getX(), vectorUnfiltered.getY()); - } - - public void update(double xUnfiltered, double yUnfiltered) - { - if (!hasBeenCalled.getBooleanValue() || containsNaN()) - { - hasBeenCalled.set(true); - set(xUnfiltered, yUnfiltered); - } - - if (maxRateVariable.getValue() < 0) - throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); - - differenceVector.setToZero(getReferenceFrame()); - differenceVector.set(xUnfiltered, yUnfiltered); - differenceVector.sub(getX(), getY()); - - limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); - add(differenceVector); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java deleted file mode 100644 index 72b6f523..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFramePose.java +++ /dev/null @@ -1,110 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.geometry.tools.EuclidGeometryIOTools; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePose3DBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameQuaternionBasics; -import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; - -public class RateLimitedYoFramePose implements FixedFramePose3DBasics -{ - private final RateLimitedYoFramePoint position; - private final RateLimitedYoFrameQuaternion orientation; - - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); - maxRate.set(initialValue); - return maxRate; - } - - public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - FramePose3DReadOnly rawPose) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, rawPose, rawPose.getReferenceFrame()); - } - - public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); - } - - public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, FramePose3DReadOnly rawPose) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPose, rawPose.getReferenceFrame()); - } - - public RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); - } - - private RateLimitedYoFramePose(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - FramePose3DReadOnly rawPose, ReferenceFrame referenceFrame) - { - if (rawPose != null) - { - this.position = new RateLimitedYoFramePoint(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, rawPose.getPosition()); - this.orientation = new RateLimitedYoFrameQuaternion(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, rawPose.getOrientation()); - } - else - { - this.position = new RateLimitedYoFramePoint(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, referenceFrame); - this.orientation = new RateLimitedYoFrameQuaternion(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, referenceFrame); - } - - reset(); - } - - public void reset() - { - position.reset(); - orientation.reset(); - } - - public void update() - { - position.update(); - orientation.update(); - - set(position, orientation); - } - - public void update(FramePose3DReadOnly framePoseUnfiltered) - { - checkReferenceFrameMatch(framePoseUnfiltered); - position.update(framePoseUnfiltered.getPosition()); - orientation.update(framePoseUnfiltered.getOrientation()); - - set(position, orientation); - } - - @Override - public FixedFramePoint3DBasics getPosition() - { - return position; - } - - @Override - public FixedFrameQuaternionBasics getOrientation() - { - return orientation; - } - - @Override - public ReferenceFrame getReferenceFrame() - { - return position.getReferenceFrame(); - } - - @Override - public String toString() - { - return EuclidGeometryIOTools.getPose3DString(this) + "-" + getReferenceFrame(); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java deleted file mode 100644 index 37a8c317..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameQuaternion.java +++ /dev/null @@ -1,151 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class RateLimitedYoFrameQuaternion extends YoFrameQuaternion -{ - private final QuaternionReadOnly rawQuaternion; - private final YoBoolean hasBeenCalled; - private final double dt; - - private final YoBoolean limited; - private final DoubleProvider maxRateVariable; - - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); - maxRate.set(initialValue); - return maxRate; - } - - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, - FrameQuaternionReadOnly rawQuaternion) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawQuaternion.getReferenceFrame(), - rawQuaternion); - } - - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, - ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, referenceFrame, null); - } - - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, - ReferenceFrame referenceFrame, QuaternionReadOnly rawQuaternion) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, referenceFrame, rawQuaternion); - } - - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - FrameQuaternionReadOnly rawQuaternion) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, rawQuaternion.getReferenceFrame(), rawQuaternion); - } - - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, referenceFrame, null); - } - - public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - ReferenceFrame referenceFrame, QuaternionReadOnly rawQuaternion) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); - limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); - - if (maxRate == null) - maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); - - maxRateVariable = maxRate; - - this.rawQuaternion = rawQuaternion; - - this.dt = dt; - - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (rawQuaternion == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(rawQuaternion); - } - - private final Quaternion quaternion = new Quaternion(); - - public void update(FrameOrientation3DReadOnly frameOrientationUnfiltered) - { - checkReferenceFrameMatch(frameOrientationUnfiltered); - quaternion.set(frameOrientationUnfiltered); - update(quaternion); - } - - public void update(FrameQuaternionReadOnly frameQuaternionUnfiltered) - { - checkReferenceFrameMatch(frameQuaternionUnfiltered); - quaternion.set(frameQuaternionUnfiltered); - update(quaternion); - } - - private final Quaternion difference = new Quaternion(); - private final Vector3D limitedRotationVector = new Vector3D(); - - public void update(QuaternionReadOnly quaternionUnfiltered) - { - if (!hasBeenCalled.getBooleanValue() || containsNaN()) - { - hasBeenCalled.set(true); - limited.set(false); - set(quaternionUnfiltered); - return; - } - - if (dot(quaternionUnfiltered) > 0.0) - { - difference.difference(this, quaternionUnfiltered); - } - else - { - difference.setAndNegate(quaternionUnfiltered); - difference.preMultiplyConjugateOther(this); - } - - difference.getRotationVector(limitedRotationVector); - boolean clipped = limitedRotationVector.clipToMaxLength(dt * maxRateVariable.getValue()); - limited.set(clipped); - - if (clipped) - { - difference.setRotationVector(limitedRotationVector); - multiply(difference); - } - else - { - set(quaternionUnfiltered); - } - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java deleted file mode 100644 index 30dc9e1c..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector.java +++ /dev/null @@ -1,120 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.FrameVector3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class RateLimitedYoFrameVector extends YoFrameVector3D -{ - private final DoubleProvider maxRateVariable; - - private final FrameTuple3DReadOnly rawPosition; - private final YoBoolean limited; - private final YoBoolean hasBeenCalled; - private final double dt; - - private final FrameVector3D differenceVector = new FrameVector3D(); - - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); - maxRate.set(initialValue); - return maxRate; - } - - public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - FrameTuple3DReadOnly rawPosition) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); - } - - public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); - } - - public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, - FrameTuple3DReadOnly rawPosition) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, - rawPosition.getReferenceFrame()); - } - - public RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); - } - - private RateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) - { - super(namePrefix, nameSuffix, referenceFrame, registry); - - this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); - this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); - - if (maxRate == null) - maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); - - maxRateVariable = maxRate; - - this.rawPosition = rawPosition; - - this.dt = dt; - - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (rawPosition == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(rawPosition); - } - - public void update(FrameTuple3DReadOnly frameVectorUnfiltered) - { - checkReferenceFrameMatch(frameVectorUnfiltered); - update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); - } - - public void update(Tuple3DReadOnly vectorUnfiltered) - { - update(vectorUnfiltered.getX(), vectorUnfiltered.getY(), vectorUnfiltered.getZ()); - } - - public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) - { - if (!hasBeenCalled.getBooleanValue() || containsNaN()) - { - hasBeenCalled.set(true); - set(xUnfiltered, yUnfiltered, zUnfiltered); - } - - if (maxRateVariable.getValue() < 0) - throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); - - differenceVector.setToZero(getReferenceFrame()); - differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); - differenceVector.sub(getX(), getY(), getZ()); - - limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); - add(differenceVector); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java deleted file mode 100644 index 2c32bbbb..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoFrameVector2d.java +++ /dev/null @@ -1,102 +0,0 @@ -package us.ihmc.robotics.math.filters; - - -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.tools.YoGeometryNameTools; -import us.ihmc.yoVariables.variable.YoDouble; - - -public class RateLimitedYoFrameVector2d extends YoFrameVector2D -{ - private final RateLimitedYoVariable x, y; - - private RateLimitedYoFrameVector2d(RateLimitedYoVariable x, RateLimitedYoVariable y, ReferenceFrame referenceFrame) - { - super(x, y, referenceFrame); - - this.x = x; - this.y = y; - } - - public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, - double maxRate, double dt, ReferenceFrame referenceFrame) - { - RateLimitedYoVariable x = new RateLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); - RateLimitedYoVariable y = new RateLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); - - RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, referenceFrame); - - return ret; - } - - public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, - YoDouble maxRate, double dt, ReferenceFrame referenceFrame) - { - RateLimitedYoVariable x = new RateLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); - RateLimitedYoVariable y = new RateLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); - - RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, referenceFrame); - - return ret; - } - - - public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, - double maxRate, double dt, YoFrameVector2D unfilteredVector) - { - RateLimitedYoVariable x = new RateLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); - RateLimitedYoVariable y = new RateLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); - - RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); - - return ret; - } - - - public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoRegistry registry, - DoubleProvider maxRate, double dt, YoFrameVector2D unfilteredVector) - { - RateLimitedYoVariable x = new RateLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); - RateLimitedYoVariable y = new RateLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); - - RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); - - return ret; - } - - public void update() - { - x.update(); - y.update(); - } - - public void update(double xUnfiltered, double yUnfiltered) - { - x.update(xUnfiltered); - y.update(yUnfiltered); - } - - public void update(Vector2DReadOnly vector2dUnfiltered) - { - x.update(vector2dUnfiltered.getX()); - y.update(vector2dUnfiltered.getY()); - } - - public void update(FrameTuple2DReadOnly vector2dUnfiltered) - { - x.update(vector2dUnfiltered.getX()); - y.update(vector2dUnfiltered.getY()); - } - - public void reset() - { - x.reset(); - y.reset(); - } -} - diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java deleted file mode 100644 index 02871c05..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableFrameVector3D.java +++ /dev/null @@ -1,121 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.FrameVector3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.yoVariables.euclid.referenceFrame.YoMutableFrameVector3D; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class RateLimitedYoMutableFrameVector3D extends YoMutableFrameVector3D -{ - private final DoubleProvider maxRateVariable; - - private final FrameTuple3DReadOnly rawPosition; - private final YoBoolean limited; - private final YoBoolean hasBeenCalled; - private final double dt; - - private final FrameVector3D differenceVector = new FrameVector3D(); - - private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) - { - YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); - maxRate.set(initialValue); - return maxRate; - } - - public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - FrameTuple3DReadOnly rawPosition) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); - } - - public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); - } - - public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, - FrameTuple3DReadOnly rawPosition) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, - rawPosition.getReferenceFrame()); - } - - public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, - ReferenceFrame referenceFrame) - { - this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); - } - - private RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, - FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) - { - super(namePrefix, nameSuffix, registry, referenceFrame); - - this.hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); - this.limited = new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); - - if (maxRate == null) - maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); - - maxRateVariable = maxRate; - - this.rawPosition = rawPosition; - - this.dt = dt; - - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (rawPosition == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(rawPosition); - } - - public void update(FrameTuple3DReadOnly frameVectorUnfiltered) - { - checkReferenceFrameMatch(frameVectorUnfiltered); - update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); - } - - public void update(Tuple3DReadOnly vectorUnfiltered) - { - update(vectorUnfiltered.getX(), vectorUnfiltered.getY(), vectorUnfiltered.getZ()); - } - - public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) - { - if (!hasBeenCalled.getBooleanValue() || containsNaN()) - { - hasBeenCalled.set(true); - set(xUnfiltered, yUnfiltered, zUnfiltered); - } - - if (maxRateVariable.getValue() < 0) - throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); - - differenceVector.setToZero(getReferenceFrame()); - differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); - differenceVector.sub(getX(), getY(), getZ()); - - limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); - add(differenceVector); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java deleted file mode 100644 index 60705109..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoVariable.java +++ /dev/null @@ -1,118 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; - -public class RateLimitedYoVariable extends YoDouble -{ - private final DoubleProvider maxRateVariable; - - private final YoDouble position; - private final YoBoolean limited; - - private final double dt; - - private final YoBoolean hasBeenCalled; - - public RateLimitedYoVariable(String name, YoRegistry registry, double maxRate, double dt) - { - super(name, registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - this.limited = new YoBoolean(name + "Limited", registry); - - YoDouble maxRateVariable = new YoDouble(name + "MaxRate", registry); - maxRateVariable.set(maxRate); - this.maxRateVariable = maxRateVariable; - - this.position = null; - - this.dt = dt; - - reset(); - } - - public RateLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRateVariable, double dt) - { - super(name, registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - this.limited = new YoBoolean(name + "Limited", registry); - - this.maxRateVariable = maxRateVariable; - this.position = null; - - this.dt = dt; - reset(); - } - - public RateLimitedYoVariable(String name, YoRegistry registry, double maxRate, YoDouble positionVariable, double dt) - { - super(name, registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - this.limited = new YoBoolean(name + "Limited", registry); - - position = positionVariable; - - YoDouble maxRateVariable = new YoDouble(name + "MaxRate", registry); - maxRateVariable.set(maxRate); - this.maxRateVariable = maxRateVariable; - - this.dt = dt; - - reset(); - } - - public RateLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRateVariable, YoDouble positionVariable, double dt) - { - super(name, registry); - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - this.limited = new YoBoolean(name + "Limited", registry); - - position = positionVariable; - this.maxRateVariable = maxRateVariable; - - this.dt = dt; - - reset(); - } - - public void reset() - { - hasBeenCalled.set(false); - } - - public void update() - { - if (position == null) - { - throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(position.getDoubleValue()); - } - - public void update(double currentPosition) - { - if (!hasBeenCalled.getBooleanValue()) - { - hasBeenCalled.set(true); - set(currentPosition); - } - - if (maxRateVariable.getValue() < 0) - throw new RuntimeException("The maxRate parameter in the RateLimitedYoVariable cannot be negative."); - - double difference = currentPosition - getDoubleValue(); - if (Math.abs(difference) > maxRateVariable.getValue() * dt) - { - difference = Math.signum(difference) * maxRateVariable.getValue() * dt; - this.limited.set(true); - } - else - this.limited.set(false); - - set(getDoubleValue() + difference); - } -} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java deleted file mode 100644 index 8fc8a2a4..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RevisedBacklashCompensatingVelocityYoVariable.java +++ /dev/null @@ -1,206 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.commons.MathTools; -import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoBoolean; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoEnum; - -/** - * This does essentially the same as BacklashCompensatingVelocityYoVariable, except does the steps in a different order and - * goes sloppy when the output changes sign rather than the finite differences. This works better for a signal with noise on it, - * which might have a true velocity but due to the noise seems to switch direction often. - * - */ -public class RevisedBacklashCompensatingVelocityYoVariable extends YoDouble implements ProcessingYoVariable -{ - private final double dt; - - private final FilteredVelocityYoVariable finiteDifferenceVelocity; - - private final DoubleProvider alphaVariable; - private final YoDouble position; - - private final YoDouble lastPosition; - private final YoBoolean hasBeenCalled; - - private final YoEnum backlashState; - private final DoubleProvider slopTime; - - private final YoDouble timeSinceSloppy; - - public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, YoDouble positionVariable, - double dt, DoubleProvider slopTime, YoRegistry registry) - { - super(name, description, registry); - - finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, registry); - - - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - - backlashState = new YoEnum(name + "BacklashState", registry, BacklashState.class, true); - backlashState.set(null); - timeSinceSloppy = new YoDouble(name + "TimeSinceSloppy", registry); - - position = positionVariable; - - this.alphaVariable = alphaVariable; - this.slopTime = slopTime; - - this.dt = dt; - - lastPosition = new YoDouble(name + "_lastPosition", registry); - - reset(); - } - - public RevisedBacklashCompensatingVelocityYoVariable(String name, String description, YoDouble alphaVariable, double dt, YoDouble slopTime, - YoRegistry registry) - { - super(name, description, registry); - - finiteDifferenceVelocity = new FilteredVelocityYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, registry); - - this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); - - backlashState = new YoEnum(name + "BacklashState", registry, BacklashState.class, true); - backlashState.set(null); - timeSinceSloppy = new YoDouble(name + "timeInState", registry); - - this.position = null; - - this.alphaVariable = alphaVariable; - this.slopTime = slopTime; - - this.dt = dt; - - lastPosition = new YoDouble(name + "_lastPosition", registry); - - reset(); - } - - @Override - public void reset() - { - hasBeenCalled.set(false); - backlashState.set(null); - } - - @Override - public void update() - { - if (position == null) - { - throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " - + "position variable to call update(), otherwise use update(double)"); - } - - update(position.getDoubleValue()); - } - - public void update(double currentPosition) - { - if (backlashState.getEnumValue() == null) - { - backlashState.set(BacklashState.FORWARD_OK); - } - - finiteDifferenceVelocity.update(currentPosition); - - if (!hasBeenCalled.getBooleanValue()) - { - hasBeenCalled.set(true); - lastPosition.set(currentPosition); - set(0.0); - } - - timeSinceSloppy.add(dt); - - double velocityFromFiniteDifferences = finiteDifferenceVelocity.getDoubleValue(); - - switch (backlashState.getEnumValue()) - { - case BACKWARD_OK : - { - if (velocityFromFiniteDifferences > 0.0) - { - timeSinceSloppy.set(0.0); -// this.set(0.0); - backlashState.set(BacklashState.FORWARD_SLOP); - } - - break; - } - - case FORWARD_OK : - { - if (velocityFromFiniteDifferences < 0.0) - { - timeSinceSloppy.set(0.0); -// this.set(0.0); - backlashState.set(BacklashState.BACKWARD_SLOP); - } - - break; - } - - case BACKWARD_SLOP : - { - if (velocityFromFiniteDifferences > 0.0) - { - timeSinceSloppy.set(0.0); -// this.set(0.0); - backlashState.set(BacklashState.FORWARD_SLOP); - } - else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) - { - backlashState.set(BacklashState.BACKWARD_OK); - } - - break; - } - - case FORWARD_SLOP : - { - if (velocityFromFiniteDifferences < 0.0) - { - timeSinceSloppy.set(0.0); -// this.set(0.0); - backlashState.set(BacklashState.BACKWARD_SLOP); - } - else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) - { - backlashState.set(BacklashState.FORWARD_OK); - } - - break; - } - } - - double difference = currentPosition - lastPosition.getDoubleValue(); - - double percent = timeSinceSloppy.getDoubleValue() / slopTime.getValue(); - percent = MathTools.clamp(percent, 0.0, 1.0); - if (Double.isNaN(percent)) - percent = 1.0; - - double scaleFactor = percent; - difference = scaleFactor * difference; - - this.updateUsingDifference(difference); - lastPosition.set(currentPosition); - } - - private void updateUsingDifference(double difference) - { - double previousFilteredDerivative = getDoubleValue(); - double currentRawDerivative = difference / dt; - - double alpha = alphaVariable.getValue(); - this.set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); - } - - private enum BacklashState {BACKWARD_OK, FORWARD_OK, BACKWARD_SLOP, FORWARD_SLOP;} -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java deleted file mode 100644 index 7b328c2d..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoFrameVector.java +++ /dev/null @@ -1,97 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.FrameVector3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.tools.YoGeometryNameTools; - -public class SecondOrderFilteredYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable -{ - private final SecondOrderFilteredYoVariable x, y, z; - - private SecondOrderFilteredYoFrameVector(SecondOrderFilteredYoVariable x, SecondOrderFilteredYoVariable y, SecondOrderFilteredYoVariable z, - ReferenceFrame referenceFrame) - { - super(x, y, z, referenceFrame); - - this.x = x; - this.y = y; - this.z = z; - } - - public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, - double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType, ReferenceFrame referenceFrame) - { - SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, registry, naturalFrequencyInHz, - dampingRatio, filterType); - return createSecondOrderFilteredYoFrameVector(namePrefix, nameSuffix, registry, dt, parameters, referenceFrame); - } - - public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, - double dt, SecondOrderFilteredYoVariableParameters parameters, ReferenceFrame referenceFrame) - { - SecondOrderFilteredYoVariable x, y, z; - x = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, dt, parameters); - y = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, dt, parameters); - z = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), registry, dt, parameters); - return new SecondOrderFilteredYoFrameVector(x, y, z, referenceFrame); - } - - public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, - double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType, YoFrameVector3D unfilteredVector) - { - SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, registry, naturalFrequencyInHz, - dampingRatio, filterType); - return createSecondOrderFilteredYoFrameVector(namePrefix, nameSuffix, registry, dt, parameters, unfilteredVector); - } - - public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoRegistry registry, - double dt, SecondOrderFilteredYoVariableParameters parameters, YoFrameVector3D unfilteredVector) - { - SecondOrderFilteredYoVariable x, y, z; - x = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoX()); - y = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoY()); - z = new SecondOrderFilteredYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoZ()); - return new SecondOrderFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); - } - - @Override - public void update() - { - x.update(); - y.update(); - z.update(); - } - - public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) - { - x.update(xUnfiltered); - y.update(yUnfiltered); - z.update(zUnfiltered); - } - - public void update(Vector3D vectorUnfiltered) - { - x.update(vectorUnfiltered.getX()); - y.update(vectorUnfiltered.getY()); - z.update(vectorUnfiltered.getZ()); - } - - public void update(FrameVector3D vectorUnfiltered) - { - checkReferenceFrameMatch(vectorUnfiltered); - x.update(vectorUnfiltered.getX()); - y.update(vectorUnfiltered.getY()); - z.update(vectorUnfiltered.getZ()); - } - - @Override - public void reset() - { - x.reset(); - y.reset(); - z.reset(); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java deleted file mode 100644 index d40f88c9..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SecondOrderFilteredYoVariableParameters.java +++ /dev/null @@ -1,36 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; - -public class SecondOrderFilteredYoVariableParameters -{ - private final YoDouble naturalFrequencyInHz; - private final YoDouble dampingRatio; - private final SecondOrderFilterType filterType; - - public SecondOrderFilteredYoVariableParameters(String name, YoRegistry registry, double naturalFrequencyInHz, double dampingRatio, - SecondOrderFilterType filterType) - { - this.naturalFrequencyInHz = new YoDouble(name + "NaturalFrequency", registry); - this.naturalFrequencyInHz.set(naturalFrequencyInHz); - this.dampingRatio = new YoDouble(name + "DampingRatio", registry); - this.dampingRatio.set(dampingRatio); - this.filterType = filterType; - } - - public YoDouble getNaturalFrequencyInHz() - { - return naturalFrequencyInHz; - } - - public YoDouble getDampingRatio() - { - return dampingRatio; - } - - public SecondOrderFilterType getFilterType() - { - return filterType; - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java deleted file mode 100644 index efe57e2a..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoFrameVector.java +++ /dev/null @@ -1,94 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import us.ihmc.euclid.referenceFrame.FrameVector3D; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.tools.YoGeometryNameTools; - -public class SimpleMovingAverageFilteredYoFrameVector extends YoFrameVector3D implements ProcessingYoVariable -{ - private final SimpleMovingAverageFilteredYoVariable x, y, z; - - private SimpleMovingAverageFilteredYoFrameVector(SimpleMovingAverageFilteredYoVariable x, SimpleMovingAverageFilteredYoVariable y, - SimpleMovingAverageFilteredYoVariable z, ReferenceFrame referenceFrame) - { - super(x, y, z, referenceFrame); - - this.x = x; - this.y = y; - this.z = z; - } - - public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, - ReferenceFrame referenceFrame, YoRegistry registry) - { - String xName = YoGeometryNameTools.createXName(namePrefix, nameSuffix); - String yName = YoGeometryNameTools.createYName(namePrefix, nameSuffix); - String zName = YoGeometryNameTools.createZName(namePrefix, nameSuffix); - - SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, registry); - SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, registry); - SimpleMovingAverageFilteredYoVariable z = new SimpleMovingAverageFilteredYoVariable(zName, windowSize, registry); - - return new SimpleMovingAverageFilteredYoFrameVector(x, y, z, referenceFrame); - } - - public static SimpleMovingAverageFilteredYoFrameVector createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, - YoFrameVector3D unfilteredVector, YoRegistry registry) - { - String xName = YoGeometryNameTools.createXName(namePrefix, nameSuffix); - String yName = YoGeometryNameTools.createYName(namePrefix, nameSuffix); - String zName = YoGeometryNameTools.createZName(namePrefix, nameSuffix); - - SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, unfilteredVector.getYoX(), registry); - SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, unfilteredVector.getYoY(), registry); - SimpleMovingAverageFilteredYoVariable z = new SimpleMovingAverageFilteredYoVariable(zName, windowSize, unfilteredVector.getYoZ(), registry); - - return new SimpleMovingAverageFilteredYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); - } - - @Override - public void update() - { - x.update(); - y.update(); - z.update(); - } - - public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) - { - x.update(xUnfiltered); - y.update(yUnfiltered); - z.update(zUnfiltered); - } - - public void update(Vector3D vectorUnfiltered) - { - x.update(vectorUnfiltered.getX()); - y.update(vectorUnfiltered.getY()); - z.update(vectorUnfiltered.getZ()); - } - - public void update(FrameVector3D vectorUnfiltered) - { - checkReferenceFrameMatch(vectorUnfiltered); - x.update(vectorUnfiltered.getX()); - y.update(vectorUnfiltered.getY()); - z.update(vectorUnfiltered.getZ()); - } - - @Override - public void reset() - { - x.reset(); - y.reset(); - z.reset(); - } - - public boolean getHasBufferWindowFilled() - { - return x.getHasBufferWindowFilled() && y.getHasBufferWindowFilled() && z.getHasBufferWindowFilled(); - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java deleted file mode 100644 index f3945330..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/SimpleMovingAverageFilteredYoVariable.java +++ /dev/null @@ -1,87 +0,0 @@ -package us.ihmc.robotics.math.filters; - -import org.ejml.data.DMatrixRMaj; -import org.ejml.dense.row.CommonOps_DDRM; - -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoInteger; - -/** - * Filter the given yoVariable using a moving average filter. This class is NOT REWINDABLE! - */ -public class SimpleMovingAverageFilteredYoVariable extends YoDouble -{ - private final YoInteger windowSize; - private final YoDouble yoVariableToFilter; - - private final DMatrixRMaj previousUpdateValues = new DMatrixRMaj(0, 0); - private int bufferPosition = 0; - - private boolean bufferHasBeenFilled = false; - - public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoRegistry registry) - { - this(name, windowSize, null, registry); - } - - public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoDouble yoVariableToFilter, YoRegistry registry) - { - super(name, registry); - - this.yoVariableToFilter = yoVariableToFilter; - this.windowSize = new YoInteger(name + "WindowSize", registry); - this.windowSize.set(windowSize); - - previousUpdateValues.reshape(windowSize, 1); - CommonOps_DDRM.fill(previousUpdateValues, 0.0); - } - - public void setWindowSize(int windowSize) - { - this.windowSize.set(windowSize); - reset(); - } - - public void update() - { - update(yoVariableToFilter.getDoubleValue()); - } - - public void update(double value) - { - if (previousUpdateValues.getNumRows() != windowSize.getIntegerValue()) - { - reset(); - } - previousUpdateValues.set(bufferPosition, 0, value); - - bufferPosition++; - - if (bufferPosition >= windowSize.getIntegerValue()) - { - bufferPosition = 0; - bufferHasBeenFilled = true; - } - - double average = 0; - for (int i = 0; i < windowSize.getIntegerValue(); i++) - { - average += previousUpdateValues.get(i, 0); - } - - this.set(average / ((double) windowSize.getIntegerValue())); - } - - public void reset() - { - bufferPosition = 0; - bufferHasBeenFilled = false; - previousUpdateValues.reshape(windowSize.getIntegerValue(), 1); - } - - public boolean getHasBufferWindowFilled() - { - return bufferHasBeenFilled; - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java deleted file mode 100644 index df502b39..00000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/frames/YoMatrix.java +++ /dev/null @@ -1,352 +0,0 @@ -package us.ihmc.robotics.math.frames; - -import org.ejml.data.*; -import org.ejml.ops.MatrixIO; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoInteger; - -import java.io.ByteArrayOutputStream; -import java.io.PrintStream; - -/** - * YoMatrix. Object for holding a matrix of YoVariables so that Matrices can be rewound. Has a - * maximum number of rows and columns and an actual number of rows and columns. If you set with a - * smaller matrix, then the actual size will be the size of the passed in matrix. extra entries will - * be set to NaN. If you get the contents the matrix you pack must be the correct size. - * - * @author JerryPratt - */ -public class YoMatrix implements DMatrix, ReshapeMatrix -{ - // TODO: eventually consolidate YoMatrix implementations - - private static final long serialVersionUID = 2156411740647948028L; - - private final int maxNumberOfRows, maxNumberOfColumns; - - private final YoInteger numberOfRows, numberOfColumns; - private final YoDouble[][] variables; - - public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) - { - this(name, null, maxNumberOfRows, maxNumberOfColumns, null, null, registry); - } - - public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, YoRegistry registry) - { - this(name, null, maxNumberOfRows, maxNumberOfColumns, rowNames, null, registry); - } - - public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) - { - this(name, null, maxNumberOfRows, maxNumberOfColumns, rowNames, columnNames, registry); - } - - public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) - { - this(name, description, maxNumberOfRows, maxNumberOfColumns, null, null, registry); - } - - public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, YoRegistry registry) - { - this(name, description, maxNumberOfRows, maxNumberOfColumns, rowNames, null, registry); - } - - public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) - { - this.maxNumberOfRows = maxNumberOfRows; - this.maxNumberOfColumns = maxNumberOfColumns; - - this.numberOfRows = new YoInteger(name + "NumRows", registry); - this.numberOfColumns = new YoInteger(name + "NumCols", registry); - - this.numberOfRows.set(maxNumberOfRows); - this.numberOfColumns.set(maxNumberOfColumns); - - variables = new YoDouble[maxNumberOfRows][maxNumberOfColumns]; - - for (int row = 0; row < maxNumberOfRows; row++) - { - for (int column = 0; column < maxNumberOfColumns; column++) - { - switch (checkNames(rowNames, columnNames)) - { - case NONE: - { - variables[row][column] = new YoDouble(getFieldName(name, row, column), description, registry); // names are simply the row and column indices - variables[row][column].setToNaN(); - break; - } - case ROWS: - { - if (maxNumberOfColumns > 1) - throw new IllegalArgumentException( - "The YoMatrix must be a column vector if only row names are provided, else unique names cannot be generated."); - - variables[row][column] = new YoDouble(getFieldName(name, rowNames[row], ""), description, registry); // names are the row names, no column identifier - variables[row][column].setToNaN(); - break; - } - case ROWS_AND_COLUMNS: - { - variables[row][column] = new YoDouble(getFieldName(name, rowNames[row], columnNames[column]), description, registry); // names are the row and column names - variables[row][column].setToNaN(); - break; - } - } - } - } - } - - public static String getFieldName(String prefix, int row, int column) - { - return getFieldName(prefix, "_" + row, "_" + column); - } - - public static String getFieldName(String prefix, String rowName, String columName) - { - return prefix + rowName + columName; - } - - private enum NamesProvided - { - NONE, ROWS, ROWS_AND_COLUMNS - } - - private NamesProvided checkNames(String[] rowNames, String[] columnNames) - { - if (rowNames == null && columnNames == null) - return NamesProvided.NONE; - else if (rowNames != null && columnNames == null) - return NamesProvided.ROWS; - else - return NamesProvided.ROWS_AND_COLUMNS; - } - - @Override - public double get(int row, int col) - { - if (col < 0 || col >= getNumCols() || row < 0 || row >= getNumRows()) - throw new IllegalArgumentException("Specified element is out of bounds: (" + row + " , " + col + ")"); - return unsafe_get(row, col); - } - - @Override - public double unsafe_get(int row, int col) - { - return variables[row][col].getValue(); - } - - @Override - public void set(int row, int col, double val) - { - if (col < 0 || col >= getNumCols() || row < 0 || row >= getNumRows()) - throw new IllegalArgumentException("Specified element is out of bounds: (" + row + " , " + col + ")"); - unsafe_set(row, col, val); - } - - @Override - public void unsafe_set(int row, int col, double val) - { - unsafe_set(row, col, val, true); - } - - private void unsafe_set(int row, int col, double val, boolean notifyListeners) - { - variables[row][col].set(val, notifyListeners); - } - - @Override - public int getNumElements() - { - return numberOfRows.getValue() * numberOfColumns.getValue(); - } - - @Override - public int getNumRows() - { - return numberOfRows.getValue(); - } - - @Override - public int getNumCols() - { - return numberOfColumns.getValue(); - } - - @Override - public void zero() - { - for (int row = 0; row < getNumRows(); row++) - { - for (int col = 0; col < getNumCols(); col++) - { - variables[row][col].set(0.0); - } - } - } - - @Override - public T copy() - { - throw new UnsupportedOperationException(); - } - - @Override - public T createLike() - { - throw new UnsupportedOperationException(); - } - - @Override - public T create(int numRows, int numCols) - { - throw new UnsupportedOperationException(); - } - - @Override - public void set(Matrix original) - { - if (original instanceof DMatrix otherMatrix) - { - reshape(otherMatrix.getNumRows(), otherMatrix.getNumCols()); - for (int row = 0; row < getNumRows(); row++) - { - for (int col = 0; col < getNumCols(); col++) - { - unsafe_set(row, col, otherMatrix.unsafe_get(row, col), false); - } - } - } - } - - @Override - public void print() - { - MatrixIO.printFancy(System.out, this, MatrixIO.DEFAULT_LENGTH); - } - - @Override - public void print(String format) - { - MatrixIO.print(System.out, this, format); - } - - @Override - public MatrixType getType() - { - return MatrixType.UNSPECIFIED; - } - - @Override - public void reshape(int numRows, int numCols) - { - if (numRows > maxNumberOfRows) - throw new IllegalArgumentException("Too many rows. Expected less or equal to " + maxNumberOfRows + ", was " + numRows); - else if (numCols > maxNumberOfColumns) - throw new IllegalArgumentException("Too many columns. Expected less or equal to " + maxNumberOfColumns + ", was " + numCols); - else if (numRows < 0 || numCols < 0) - throw new IllegalArgumentException("Cannot reshape with a negative number of rows or columns."); - - numberOfRows.set(numRows); - numberOfColumns.set(numCols); - - for (int row = 0; row < numRows; row++) - { - for (int col = numCols; col < maxNumberOfColumns; col++) - { - unsafe_set(row, col, Double.NaN, false); - } - } - - for (int row = numRows; row < maxNumberOfRows; row++) - { - for (int col = 0; col < maxNumberOfColumns; col++) - { - unsafe_set(row, col, Double.NaN, false); - } - } - } - - public void set(DMatrix matrix) - { - int numRows = matrix.getNumRows(); - int numCols = matrix.getNumCols(); - - if (((numRows > maxNumberOfRows) || (numCols > maxNumberOfColumns)) && (numRows > 0) && (numCols > 0)) - throw new RuntimeException("Not enough rows or columns. matrix to set is " + matrix.getNumRows() + " by " + matrix.getNumCols()); - - this.numberOfRows.set(numRows); - this.numberOfColumns.set(numCols); - - for (int row = 0; row < maxNumberOfRows; row++) - { - for (int column = 0; column < maxNumberOfColumns; column++) - { - double value; - if ((row < numRows) && (column < numCols)) - { - value = matrix.unsafe_get(row, column); - } - else - { - value = Double.NaN; - } - unsafe_set(row, column, value, false); - - } - } - } - - public void getAndReshape(DMatrixRMaj matrixToPack) - { - matrixToPack.reshape(getNumRows(), getNumCols()); - get(matrixToPack); - } - - public void get(DMatrix matrixToPack) - { - int numRows = matrixToPack.getNumRows(); - int numCols = matrixToPack.getNumCols(); - - if (((numRows > maxNumberOfRows) || (numCols > maxNumberOfColumns)) && (numRows > 0) && (numCols > 0)) - throw new RuntimeException("Not enough rows or columns. matrixToPack is " + matrixToPack.getNumRows() + " by " + matrixToPack.getNumCols()); - if ((numRows != this.numberOfRows.getIntegerValue()) || (numCols != this.numberOfColumns.getIntegerValue())) - throw new RuntimeException("Numer of rows and columns must be the same. Call getAndReshape() if you want to reshape the matrixToPack"); - - for (int row = 0; row < numRows; row++) - { - for (int column = 0; column < numCols; column++) - { - matrixToPack.unsafe_set(row, column, variables[row][column].getDoubleValue()); - } - } - } - - public void setToNaN(int numberOfRows, int numberOfColumns) - { - reshape(numberOfRows, numberOfColumns); - for (int row = 0; row < numberOfRows; row++) - { - for (int col = 0; col < numberOfColumns; col++) - { - unsafe_set(row, col, Double.NaN, false); - } - } - } - - public YoDouble getYoDouble(int row, int col) - { - return variables[row][col]; - } - - @Override - public String toString() - { - ByteArrayOutputStream stream = new ByteArrayOutputStream(); - MatrixIO.print(new PrintStream(stream), this); - - return stream.toString(); - } -} From f7d6932b8f46f387c03c3659722c719ca5560b8d Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Wed, 16 Oct 2024 14:01:36 -0500 Subject: [PATCH 39/47] cleaned up the ihmc math package for extraction prepped the yo variables by moving them into their own packages --- ihmc-yovariables-filters/LICENSE.txt | 202 +++++++++ ihmc-yovariables-filters/build.gradle.kts | 23 + ihmc-yovariables-filters/gradle.properties | 6 + ihmc-yovariables-filters/settings.gradle.kts | 23 + .../AccelerationLimitedYoFrameVector3D.java | 160 +++++++ .../AlphaFilteredRigidBodyTransform.java | 43 ++ .../euclid/filters/AlphaFilteredTuple2D.java | 108 +++++ .../euclid/filters/AlphaFilteredTuple3D.java | 135 ++++++ .../filters/AlphaFilteredYoFramePoint2D.java | 92 ++++ .../filters/AlphaFilteredYoFramePoint3D.java | 90 ++++ .../filters/AlphaFilteredYoFramePose3D.java | 112 +++++ .../AlphaFilteredYoFrameQuaternion.java | 95 +++++ .../filters/AlphaFilteredYoFrameVector2D.java | 90 ++++ .../filters/AlphaFilteredYoFrameVector3D.java | 100 +++++ .../AlphaFilteredYoMutableFrameVector3D.java | 89 ++++ ...shCompensatingVelocityYoFrameVector3D.java | 71 ++++ .../BacklashProcessingYoFrameVector3D.java | 57 +++ .../filters/DeadbandedYoFrameVector3D.java | 57 +++ ...lteredFiniteDifferenceYoFrameVector2D.java | 113 +++++ ...lteredFiniteDifferenceYoFrameVector3D.java | 113 +++++ ...ferenceAngularVelocityYoFrameVector3D.java | 98 +++++ .../filters/MovingAverageYoFramePoint2D.java | 65 +++ .../filters/MovingAverageYoFrameVector2D.java | 65 +++ .../RateLimitedYoFrameOrientation.java | 145 +++++++ .../filters/RateLimitedYoFramePoint2D.java | 135 ++++++ .../filters/RateLimitedYoFramePoint3D.java | 113 +++++ .../filters/RateLimitedYoFramePose3D.java | 103 +++++ .../filters/RateLimitedYoFrameQuaternion.java | 144 +++++++ .../filters/RateLimitedYoFrameVector2D.java | 48 +++ .../filters/RateLimitedYoFrameVector3D.java | 114 +++++ .../RateLimitedYoMutableFrameVector3D.java | 95 +++++ .../RunningAverageYoFrameVector3D.java | 78 ++++ .../SecondOrderFilteredYoFrameVector3D.java | 129 ++++++ ...eMovingAverageFilteredYoFrameVector3D.java | 96 +++++ .../AccelerationLimitedYoVariable.java | 138 ++++++ .../AlphaBasedOnBreakFrequencyProvider.java | 55 +++ .../yoVariables/filters/AlphaFilterTools.java | 33 ++ .../AlphaFilteredWrappingYoVariable.java | 139 ++++++ .../filters/AlphaFilteredYoMatrix.java | 108 +++++ .../filters/AlphaFilteredYoVariable.java | 172 ++++++++ .../ihmc/yoVariables/filters/AngleTools.java | 399 +++++++++++++++++ ...acklashCompensatingVelocityYoVariable.java | 202 +++++++++ .../filters/BacklashProcessingYoVariable.java | 150 +++++++ .../yoVariables/filters/BacklashState.java | 18 + .../yoVariables/filters/DeadbandTools.java | 140 ++++++ .../filters/DeadbandedYoVariable.java | 59 +++ .../yoVariables/filters/DelayedYoBoolean.java | 72 ++++ .../yoVariables/filters/DelayedYoDouble.java | 59 +++ .../filters/DeltaLimitedYoVariable.java | 68 +++ .../FilteredDiscreteVelocityYoVariable.java | 168 ++++++++ .../FilteredFiniteDifferenceYoVariable.java | 140 ++++++ .../FirstOrderBandPassFilteredYoDouble.java | 185 ++++++++ .../filters/FirstOrderFilteredYoDouble.java | 140 ++++++ .../filters/GlitchFilteredYoBoolean.java | 92 ++++ .../filters/GlitchFilteredYoInteger.java | 99 +++++ .../filters/JerkLimitedYoDouble.java | 151 +++++++ .../filters/MovingAverageYoDouble.java | 114 +++++ .../filters/ProcessingYoVariable.java | 10 + .../filters/RateLimitedYoVariable.java | 87 ++++ .../filters/RunningAverageYoDouble.java | 52 +++ .../filters/SecondOrderFilterType.java | 6 + .../filters/SecondOrderFilteredYoDouble.java | 204 +++++++++ ...condOrderFilteredYoVariableParameters.java | 36 ++ ...SimpleMovingAverageFilteredYoVariable.java | 87 ++++ .../yoVariables/filters/VariableTools.java | 48 +++ .../us/ihmc/yoVariables/filters/YoMatrix.java | 352 +++++++++++++++ .../AlphaFilteredRigidBodyTransformTest.java | 152 +++++++ .../filters/AlphaFilteredTuple3DTest.java | 106 +++++ .../AlphaFilteredYoFramePoint2DTest.java | 54 +++ .../AlphaFilteredYoFramePoint3DTest.java | 56 +++ .../AlphaFilteredYoFrameQuaternionTest.java | 158 +++++++ .../AlphaFilteredYoFrameVector2DTest.java | 54 +++ .../AlphaFilteredYoFrameVector3DTest.java | 57 +++ .../filters/EuclidCoreTestMissingTools.java | 34 ++ .../FilteredVelocityYoFrameVector2DTest.java | 55 +++ .../FilteredVelocityYoFrameVector3DTest.java | 57 +++ .../RateLimitedYoFrameOrientationTest.java | 81 ++++ .../RateLimitedYoFrameQuaternionTest.java | 79 ++++ .../RunningAverageYoFrameVector3DTest.java | 44 ++ .../AlphaFilteredWrappingYoVariableTest.java | 218 ++++++++++ .../filters/AlphaFilteredYoVariableTest.java | 64 +++ ...ashCompensatingVelocityYoVariableTest.java | 373 ++++++++++++++++ .../BacklashProcessingYoVariableTest.java | 46 ++ .../filters/DeadbandedYoVariableTest.java | 59 +++ .../filters/DelayedYoBooleanTest.java | 190 +++++++++ .../filters/DelayedYoDoubleTest.java | 121 ++++++ .../filters/DeltaLimitedYoVariableTest.java | 400 ++++++++++++++++++ ...ilteredDiscreteVelocityYoVariableTest.java | 70 +++ .../FilteredVelocityYoVariableTest.java | 67 +++ .../FirstOrderFilteredYoDoubleTest.java | 239 +++++++++++ .../filters/GlitchFilteredYoBooleanTest.java | 152 +++++++ .../filters/MovingAverageYoDoubleTest.java | 71 ++++ .../filters/RateLimitedYoVariableTest.java | 183 ++++++++ .../filters/RunningAverageYoDoubleTest.java | 29 ++ .../SecondOrderFilteredYoDoubleTest.java | 68 +++ ...leMovingAverageFilteredYoVariableTest.java | 39 ++ 96 files changed, 10366 insertions(+) create mode 100644 ihmc-yovariables-filters/LICENSE.txt create mode 100644 ihmc-yovariables-filters/build.gradle.kts create mode 100644 ihmc-yovariables-filters/gradle.properties create mode 100644 ihmc-yovariables-filters/settings.gradle.kts create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AngleTools.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashState.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandTools.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilterType.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/VariableTools.java create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/YoMatrix.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2DTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2DTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/EuclidCoreTestMissingTools.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector2DTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector3DTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariableTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariableTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariableTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariableTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeadbandedYoVariableTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoBooleanTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoDoubleTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariableTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredVelocityYoVariableTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBooleanTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RateLimitedYoVariableTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RunningAverageYoDoubleTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java diff --git a/ihmc-yovariables-filters/LICENSE.txt b/ihmc-yovariables-filters/LICENSE.txt new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/ihmc-yovariables-filters/LICENSE.txt @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/ihmc-yovariables-filters/build.gradle.kts b/ihmc-yovariables-filters/build.gradle.kts new file mode 100644 index 00000000..c218255d --- /dev/null +++ b/ihmc-yovariables-filters/build.gradle.kts @@ -0,0 +1,23 @@ +plugins { + id("us.ihmc.ihmc-build") + id("us.ihmc.ihmc-ci") version "8.3" + id("us.ihmc.ihmc-cd") version "1.26" + id("us.ihmc.log-tools-plugin") version "0.6.3" +} + +ihmc { + loadProductProperties("../product.properties") + + configureDependencyResolution() + configurePublications() +} + +mainDependencies { + api("us.ihmc:ihmc-yovariables:0.12.2") + api("org.ejml:ejml-ddense:0.39") +} + +testDependencies { + api("us.ihmc:ihmc-yovariables-test:0.12.2") + api("org.apache.commons:commons-math3:3.3") +} diff --git a/ihmc-yovariables-filters/gradle.properties b/ihmc-yovariables-filters/gradle.properties new file mode 100644 index 00000000..d883b77c --- /dev/null +++ b/ihmc-yovariables-filters/gradle.properties @@ -0,0 +1,6 @@ +kebabCasedName = ihmc-yovariables-filters +pascalCasedName = IHMCYoVariablesFilters +extraSourceSets = ["test"] +publishUrl = local +compositeSearchHeight = 2 +excludeFromCompositeBuild = false \ No newline at end of file diff --git a/ihmc-yovariables-filters/settings.gradle.kts b/ihmc-yovariables-filters/settings.gradle.kts new file mode 100644 index 00000000..31528b14 --- /dev/null +++ b/ihmc-yovariables-filters/settings.gradle.kts @@ -0,0 +1,23 @@ +pluginManagement { + plugins { + id("us.ihmc.ihmc-build") version "0.29.7" + } +} + +buildscript { + repositories { + maven { url = uri("https://plugins.gradle.org/m2/") } + mavenLocal() + } + dependencies { + classpath("us.ihmc:ihmc-build:0.29.7") + } +} + +/** + * Browse source at https://github.com/ihmcrobotics/ihmc-build + */ +val ihmcSettingsConfigurator = us.ihmc.build.IHMCSettingsConfigurator(settings, logger, extra) +ihmcSettingsConfigurator.checkRequiredPropertiesAreSet() +ihmcSettingsConfigurator.configureExtraSourceSets() +ihmcSettingsConfigurator.findAndIncludeCompositeBuilds() diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java new file mode 100644 index 00000000..ff2e7293 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java @@ -0,0 +1,160 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class AccelerationLimitedYoFrameVector3D extends YoFrameVector3D +{ + private final DoubleProvider maxRateVariable; + private final DoubleProvider maxAccelerationVariable; + + private final FrameTuple3DReadOnly rawPosition; + private final YoBoolean accelerationLimited; + private final YoBoolean rateLimited; + + private final YoBoolean hasBeenInitialized; + private final YoDouble positionGain; + private final YoDouble velocityGain; + + private final YoFrameVector3D smoothedRate; + private final YoFrameVector3D smoothedAcceleration; + + private final double dt; + + private final FrameVector3D positionError; + private final FrameVector3D acceleration; + + public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, + DoubleProvider maxAcceleration, double dt, FrameTuple3DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, maxRate, maxAcceleration, dt, rawPosition, rawPosition.getReferenceFrame()); + } + + public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, + DoubleProvider maxAcceleration, double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, maxAcceleration, dt, null, referenceFrame); + } + public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double maxAcceleration, + double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), + VariableTools.createMaxAccelerationYoDouble(namePrefix, nameSuffix, maxAcceleration, registry), dt, null, referenceFrame); + } + + private AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, + DoubleProvider maxAcceleration, double dt, FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + positionError = new FrameVector3D(referenceFrame); + acceleration = new FrameVector3D(referenceFrame); + + if (maxRate == null) + maxRate = VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + if (maxAcceleration == null) + maxAcceleration = VariableTools.createMaxAccelerationYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + this.maxRateVariable = maxRate; + this.maxAccelerationVariable = maxAcceleration; + + this.dt = dt; + + hasBeenInitialized = new YoBoolean(namePrefix + "HasBeenInitialized" + namePrefix, registry); + this.rateLimited = new YoBoolean(namePrefix + "RateLimited" + nameSuffix, registry); + this.accelerationLimited = new YoBoolean(namePrefix + "AccelerationLimited" + nameSuffix, registry); + + smoothedRate = new YoFrameVector3D(namePrefix + "SmoothedRate" + namePrefix, referenceFrame, registry); + smoothedAcceleration = new YoFrameVector3D(namePrefix + "SmoothedAcceleration" + namePrefix, referenceFrame, registry); + + positionGain = new YoDouble(namePrefix + "PositionGain" + namePrefix, registry); + velocityGain = new YoDouble(namePrefix + "VelocityGain" + namePrefix, registry); + + double w0 = 2.0 * Math.PI * 16.0; + double zeta = 1.0; + + setGainsByPolePlacement(w0, zeta); + hasBeenInitialized.set(false); + + this.rawPosition = rawPosition; + + } + + + public void setGainsByPolePlacement(double w0, double zeta) + { + positionGain.set(w0 * w0); + velocityGain.set(2.0 * zeta * w0); + } + + public void update() + { + if (rawPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawPosition); + } + + public void update(FrameTuple3DReadOnly frameVectorUnfiltered) + { + checkReferenceFrameMatch(frameVectorUnfiltered); + update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + if (!hasBeenInitialized.getBooleanValue()) + initialize(xUnfiltered, yUnfiltered, zUnfiltered); + + positionError.set(xUnfiltered, yUnfiltered, zUnfiltered); + positionError.sub(this); + + acceleration.set(smoothedRate); + acceleration.scale(-velocityGain.getValue()); + acceleration.scaleAdd(positionGain.getValue(), positionError, acceleration); + + accelerationLimited.set(acceleration.clipToMaxLength(maxAccelerationVariable.getValue())); + + smoothedAcceleration.set(acceleration); + smoothedRate.scaleAdd(dt, smoothedAcceleration, smoothedRate); + + rateLimited.set(smoothedRate.clipToMaxLength(maxRateVariable.getValue())); + + this.scaleAdd(dt, smoothedRate, this); + + if (this.containsNaN()) + throw new RuntimeException("what?"); + + } + + public void initialize(FrameTuple3DReadOnly input) + { + initialize(input.getX(), input.getY(), input.getZ()); + } + + public void initialize(double xInput, double yInput, double zInput) + { + this.set(xInput, yInput, zInput); + smoothedRate.setToZero(); + smoothedAcceleration.setToZero(); + + this.hasBeenInitialized.set(true); + } + + public void reset() + { + this.hasBeenInitialized.set(false); + smoothedRate.setToZero(); + smoothedAcceleration.setToZero(); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java new file mode 100644 index 00000000..448326c4 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java @@ -0,0 +1,43 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.transform.RigidBodyTransform; + +public class AlphaFilteredRigidBodyTransform extends RigidBodyTransform +{ + // an alpha of zero applies zero filtering, accepting the entirely new input. + private double alpha = 0.0; + private final RigidBodyTransform previousFiltered = new RigidBodyTransform(); + + public AlphaFilteredRigidBodyTransform() + { + reset(); + } + + public void update(RigidBodyTransform measured) + { + if (containsNaN()) + { + set(measured); + } + else + { + previousFiltered.set(this); + interpolate(measured, previousFiltered, alpha); + } + } + + public double getAlpha() + { + return alpha; + } + + public void setAlpha(double alpha) + { + this.alpha = alpha; + } + + public void reset() + { + setToNaN(); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java new file mode 100644 index 00000000..59d059c0 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java @@ -0,0 +1,108 @@ +package us.ihmc.yoVariables.euclid.filters; + +import org.apache.commons.lang3.NotImplementedException; + +import us.ihmc.euclid.interfaces.EuclidGeometry; +import us.ihmc.euclid.tools.EuclidCoreTools; +import us.ihmc.euclid.transform.interfaces.Transform; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; +import us.ihmc.yoVariables.providers.DoubleProvider; + +public class AlphaFilteredTuple2D implements Tuple2DBasics +{ + private final DoubleProvider alpha; + + private boolean resetX; + private boolean resetY; + + private double x; + private double y; + + public AlphaFilteredTuple2D(DoubleProvider alpha) + { + this.alpha = alpha; + reset(); + } + + public AlphaFilteredTuple2D(Tuple2DReadOnly initialValue, DoubleProvider alpha) + { + this.alpha = alpha; + reset(initialValue); + } + + public void reset() + { + resetX = true; + resetY = true; + } + + public void reset(Tuple2DReadOnly other) + { + reset(other.getX(), other.getY()); + } + + public void reset(double x, double y) + { + reset(); + set(x, y); + } + + @Override + public double getX() + { + return x; + } + + @Override + public double getY() + { + return y; + } + + @Override + public void setX(double x) + { + if (resetX) + { + this.x = x; + resetX = false; + } + else + { + this.x = EuclidCoreTools.interpolate(x, this.x, alpha.getValue()); + } + } + + @Override + public void setY(double y) + { + if (resetY) + { + this.y = y; + resetY = false; + } + else + { + this.y = EuclidCoreTools.interpolate(y, this.y, alpha.getValue()); + } + } + + @Override + public boolean geometricallyEquals(EuclidGeometry geometry, double epsilon) + { + return epsilonEquals(geometry, epsilon); + } + + @Override + public void applyTransform(Transform transform, boolean checkIfTransformInXYPlane) + { + throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + "."); + } + + @Override + public void applyInverseTransform(Transform transform, boolean checkIfTransformInXYPlane) + { + throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + "."); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java new file mode 100644 index 00000000..e2010930 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java @@ -0,0 +1,135 @@ +package us.ihmc.yoVariables.euclid.filters; + +import org.apache.commons.lang3.NotImplementedException; +import us.ihmc.euclid.interfaces.EuclidGeometry; +import us.ihmc.euclid.tools.EuclidCoreTools; +import us.ihmc.euclid.transform.interfaces.Transform; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.providers.DoubleProvider; + +public class AlphaFilteredTuple3D implements Tuple3DBasics +{ + private final DoubleProvider alpha; + + private double x = Double.NaN; + private double y = Double.NaN; + private double z = Double.NaN; + + private boolean resetX = false; + private boolean resetY = false; + private boolean resetZ = false; + + public AlphaFilteredTuple3D(DoubleProvider alpha) + { + this.alpha = alpha; + } + + public AlphaFilteredTuple3D(double x, double y, double z, DoubleProvider alpha) + { + this.alpha = alpha; + reset(x, y, z); + } + + public AlphaFilteredTuple3D(Tuple3DReadOnly other, DoubleProvider alpha) + { + this.alpha = alpha; + reset(other); + } + + public void reset() + { + resetX = true; + resetY = true; + resetZ = true; + } + + public void reset(Tuple3DReadOnly other) + { + reset(other.getX(), other.getY(), other.getZ()); + } + + public void reset(double x, double y, double z) + { + reset(); + set(x, y, z); + } + + @Override + public void setX(double x) + { + if (resetX || Double.isNaN(this.x)) + { + this.x = x; + resetX = false; + } + else + { + this.x = EuclidCoreTools.interpolate(x, this.x, alpha.getValue()); + } + } + + @Override + public void setY(double y) + { + if (resetY || Double.isNaN(this.y)) + { + this.y = y; + resetY = false; + } + else + { + this.y = EuclidCoreTools.interpolate(y, this.y, alpha.getValue()); + } + } + + @Override + public void setZ(double z) + { + if (resetZ || Double.isNaN(this.z)) + { + this.z = z; + resetZ = false; + } + else + { + this.z = EuclidCoreTools.interpolate(z, this.z, alpha.getValue()); + } + } + + @Override + public double getX() + { + return x; + } + + @Override + public double getY() + { + return y; + } + + @Override + public double getZ() + { + return z; + } + + @Override + public boolean geometricallyEquals(EuclidGeometry geometry, double epsilon) + { + return epsilonEquals(geometry, epsilon); + } + + @Override + public void applyTransform(Transform transform) + { + throw new NotImplementedException("Sorry mate, " + getClass().getSimpleName() + " doesn't implement this method."); + } + + @Override + public void applyInverseTransform(Transform transform) + { + throw new NotImplementedException("Sorry mate, " + getClass().getSimpleName() + " doesn't implement this method."); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java new file mode 100644 index 00000000..135fe68b --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java @@ -0,0 +1,92 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; +import us.ihmc.euclid.tuple2D.Point2D; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class AlphaFilteredYoFramePoint2D extends YoFramePoint2D +{ + private final DoubleProvider alpha; + + private final FrameTuple2DReadOnly unfilteredPoint; + private final YoBoolean hasBeenCalled; + + public AlphaFilteredYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, VariableTools.createAlphaYoDouble(namePrefix, nameSuffix, alpha, registry), referenceFrame); + } + + public AlphaFilteredYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); + } + + public AlphaFilteredYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, + FrameTuple2DReadOnly unfilteredPoint) + { + this(namePrefix, nameSuffix, registry, alpha, unfilteredPoint.getReferenceFrame(), unfilteredPoint); + } + + private AlphaFilteredYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + FrameTuple2DReadOnly unfilteredPoint) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + this.alpha = alpha; + this.unfilteredPoint = unfilteredPoint; + if (unfilteredPoint != null) + checkReferenceFrameMatch(unfilteredPoint); + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (unfilteredPoint == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(unfilteredPoint); + } + + public void update(FrameTuple2DReadOnly unfilteredPoint) + { + checkReferenceFrameMatch(unfilteredPoint); + update((Tuple2DReadOnly) unfilteredPoint); + } + + public void update(Tuple2DReadOnly unfilteredPoint) + { + update(unfilteredPoint.getX(), unfilteredPoint.getY()); + } + + private final Point2D unfilteredPoint2D = new Point2D(); + + public void update(double xUnfiltered, double yUnfiltered) + { + if (!hasBeenCalled.getValue()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered); + } + else + { + unfilteredPoint2D.set(xUnfiltered, yUnfiltered); + interpolate(unfilteredPoint2D, this, alpha.getValue()); + } + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java new file mode 100644 index 00000000..16ff00eb --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java @@ -0,0 +1,90 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class AlphaFilteredYoFramePoint3D extends YoFramePoint3D +{ + private final DoubleProvider alpha; + + private final FrameTuple3DReadOnly unfilteredPoint; + private final YoBoolean hasBeenCalled; + + public AlphaFilteredYoFramePoint3D(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, VariableTools.createAlphaYoDouble(namePrefix, nameSuffix, alpha, registry), referenceFrame); + } + + public AlphaFilteredYoFramePoint3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); + } + + public AlphaFilteredYoFramePoint3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, + FrameTuple3DReadOnly unfilteredPoint) + { + this(namePrefix, nameSuffix, registry, alpha, unfilteredPoint.getReferenceFrame(), unfilteredPoint); + } + + private AlphaFilteredYoFramePoint3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + FrameTuple3DReadOnly unfilteredPoint) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + this.alpha = alpha; + this.unfilteredPoint = unfilteredPoint; + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (unfilteredPoint == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(unfilteredPoint); + } + + public void update(FrameTuple3DReadOnly unfilteredPoint) + { + checkReferenceFrameMatch(unfilteredPoint); + update((Tuple3DReadOnly) unfilteredPoint); + } + + public void update(Tuple3DReadOnly unfilteredTuple3D) + { + update(unfilteredTuple3D.getX(), unfilteredTuple3D.getY(), unfilteredTuple3D.getZ()); + } + + private final Point3D unfilteredPoint3D = new Point3D(); + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + if (!hasBeenCalled.getValue()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered, zUnfiltered); + } + else + { + unfilteredPoint3D.set(xUnfiltered, yUnfiltered, zUnfiltered); + interpolate(unfilteredPoint3D, this, alpha.getValue()); + } + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java new file mode 100644 index 00000000..8c23d2f9 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java @@ -0,0 +1,112 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.geometry.Pose3D; +import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePose3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; +import us.ihmc.yoVariables.filters.ProcessingYoVariable; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class AlphaFilteredYoFramePose3D extends YoFramePose3D implements ProcessingYoVariable +{ + private final DoubleProvider alpha; + private final FramePose3DReadOnly unfilteredPose; + private final YoBoolean hasBeenCalled; + + private final Pose3D poseMeasured = new Pose3D(); + private final Pose3D posePreviousFiltered = new Pose3D(); + + public AlphaFilteredYoFramePose3D(String namePrefix, String nameSuffix, FramePose3DReadOnly unfilteredPose, DoubleProvider alpha, YoRegistry registry) + { + this(namePrefix, nameSuffix, unfilteredPose, alpha, unfilteredPose.getReferenceFrame(), registry); + } + + private AlphaFilteredYoFramePose3D(String namePrefix, + String nameSuffix, + FramePose3DReadOnly unfilteredPose, + DoubleProvider alpha, + ReferenceFrame referenceFrame, + YoRegistry registry) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + this.unfilteredPose = unfilteredPose; + + if (alpha == null) + alpha = VariableTools.createAlphaYoDouble(namePrefix, "", 0.0, registry); + this.alpha = alpha; + + this.hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + } + + public AlphaFilteredYoFramePose3D(YoFramePoint3D position, + YoFrameQuaternion orientation, + FramePose3DReadOnly unfilteredPose, + DoubleProvider alpha, + YoRegistry registry) + { + super(position, orientation); + this.unfilteredPose = unfilteredPose; + + String namePrefix = YoGeometryNameTools.getCommonPrefix(position.getNamePrefix(), orientation.getNamePrefix()); + String nameSuffix = YoGeometryNameTools.getCommonSuffix(position.getNameSuffix(), orientation.getNameSuffix()); + + if (alpha == null) + alpha = VariableTools.createAlphaYoDouble(namePrefix, "", 0.0, registry); + this.alpha = alpha; + + this.hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + } + + @Override + public void update() + { + if (unfilteredPose == null) + { + throw new NullPointerException( + "AlphaFilteredYoFramePose3D must be constructed with a non null " + "pose variable to call update(), otherwise use update(Pose3DReadOnly)"); + } + + poseMeasured.set(unfilteredPose); + update(poseMeasured); + } + + public void update(FramePose3DReadOnly rawPose) + { + checkReferenceFrameMatch(rawPose); + poseMeasured.set(rawPose); + update(poseMeasured); + } + + public void update(Pose3DReadOnly rawPose) + { + if (hasBeenCalled.getBooleanValue()) + { + posePreviousFiltered.set(this); + + interpolate(poseMeasured, posePreviousFiltered, alpha.getValue()); // qPreviousFiltered 'gets multiplied by alpha' + } + else + { + set(poseMeasured); + hasBeenCalled.set(true); + } + } + + @Override + public void reset() + { + hasBeenCalled.set(false); + } + + public FramePose3DReadOnly getUnfilteredPose() + { + return unfilteredPose; + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java new file mode 100644 index 00000000..00912e2e --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java @@ -0,0 +1,95 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; +import us.ihmc.yoVariables.filters.ProcessingYoVariable; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class AlphaFilteredYoFrameQuaternion extends YoFrameQuaternion implements ProcessingYoVariable +{ + private final YoFrameQuaternion unfilteredQuaternion; + private final DoubleProvider alpha; + private final YoBoolean hasBeenCalled; + private final Quaternion qMeasured = new Quaternion(); + private final Quaternion qPreviousFiltered = new Quaternion(); + private final Quaternion qNewFiltered = new Quaternion(); + + public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, DoubleProvider alpha, + YoRegistry registry) + { + this(namePrefix, nameSuffix, unfilteredQuaternion, alpha, unfilteredQuaternion.getReferenceFrame(), registry); + } + + private AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, DoubleProvider alpha, + ReferenceFrame referenceFrame, YoRegistry registry) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + this.unfilteredQuaternion = unfilteredQuaternion; + + if (alpha == null) + alpha = VariableTools.createAlphaYoDouble(namePrefix, "", 0.0, registry); + this.alpha = alpha; + + this.hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + } + + @Override + public void update() + { + if (unfilteredQuaternion == null) + { + throw new NullPointerException("AlphaFilteredYoFrameQuaternion must be constructed with a non null " + + "quaternion variable to call update(), otherwise use update(Quat4d)"); + } + + qMeasured.set(unfilteredQuaternion); + update(qMeasured); + } + + public void update(FrameOrientation3DReadOnly rawOrientation) + { + checkReferenceFrameMatch(rawOrientation); + qMeasured.set(rawOrientation); + update(qMeasured); + } + + public void update(Orientation3DReadOnly rawOrientation) + { + qMeasured.set(rawOrientation); + update(qMeasured); + } + + private void update(QuaternionReadOnly qMeasured) + { + if (hasBeenCalled.getBooleanValue()) + { + qPreviousFiltered.set(this); + + qNewFiltered.interpolate(qMeasured, qPreviousFiltered, alpha.getValue()); // qPreviousFiltered 'gets multiplied by alpha' + set(qNewFiltered); + } + else + { + set(qMeasured); + hasBeenCalled.set(true); + } + } + + @Override + public void reset() + { + hasBeenCalled.set(false); + } + + public YoFrameQuaternion getUnfilteredQuaternion() + { + return unfilteredQuaternion; + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java new file mode 100644 index 00000000..690ce308 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java @@ -0,0 +1,90 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; +import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class AlphaFilteredYoFrameVector2D extends YoFrameVector2D +{ + private final DoubleProvider alpha; + + private final FrameTuple2DReadOnly unfilteredPosition; + private final YoBoolean hasBeenCalled; + + public AlphaFilteredYoFrameVector2D(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, VariableTools.createAlphaYoDouble(namePrefix, nameSuffix, alpha, registry), referenceFrame); + } + + public AlphaFilteredYoFrameVector2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); + } + + public AlphaFilteredYoFrameVector2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, + FrameTuple2DReadOnly unfilteredFrameTuple2D) + { + this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple2D.getReferenceFrame(), unfilteredFrameTuple2D); + } + + private AlphaFilteredYoFrameVector2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + FrameTuple2DReadOnly unfilteredPosition) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + this.alpha = alpha; + this.unfilteredPosition = unfilteredPosition; + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (unfilteredPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(unfilteredPosition); + } + + public void update(FrameTuple2DReadOnly unfilteredFrameTuple2D) + { + checkReferenceFrameMatch(unfilteredFrameTuple2D); + update((Tuple2DReadOnly) unfilteredFrameTuple2D); + } + + public void update(Tuple2DReadOnly unfilteredTuple2D) + { + update(unfilteredTuple2D.getX(), unfilteredTuple2D.getY()); + } + + private final Vector2D unfilteredVector2D = new Vector2D(); + + public void update(double xUnfiltered, double yUnfiltered) + { + if (!hasBeenCalled.getValue()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered); + } + else + { + unfilteredVector2D.set(xUnfiltered, yUnfiltered); + interpolate(unfilteredVector2D, this, alpha.getValue()); + } + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java new file mode 100644 index 00000000..2304541d --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java @@ -0,0 +1,100 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.filters.ProcessingYoVariable; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class AlphaFilteredYoFrameVector3D extends YoFrameVector3D implements ProcessingYoVariable +{ + private final DoubleProvider alphaProvider; + + private final FrameTuple3DReadOnly position; + private final YoBoolean hasBeenCalled; + + public AlphaFilteredYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, VariableTools.createAlphaYoDouble(namePrefix, nameSuffix, alpha, registry), referenceFrame); + } + + public AlphaFilteredYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); + } + + public AlphaFilteredYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double alpha, + FrameTuple3DReadOnly unfilteredFrameTuple3D) + { + this(namePrefix, nameSuffix, registry, VariableTools.createAlphaYoDouble(namePrefix, nameSuffix, alpha, registry), + unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); + } + + public AlphaFilteredYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, + FrameTuple3DReadOnly unfilteredFrameTuple3D) + { + this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameTuple3D.getReferenceFrame(), unfilteredFrameTuple3D); + } + + private AlphaFilteredYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, ReferenceFrame referenceFrame, + FrameTuple3DReadOnly unfilteredFrameTuple3D) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + alphaProvider = alpha; + + position = unfilteredFrameTuple3D; + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + reset(); + } + + @Override + public void reset() + { + hasBeenCalled.set(false); + } + + @Override + public void update() + { + if (position == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position); + } + + public void update(FrameTuple3DReadOnly unfilteredFrameTuple3D) + { + checkReferenceFrameMatch(unfilteredFrameTuple3D); + update((Tuple3DReadOnly) unfilteredFrameTuple3D); + } + + public void update(Tuple3DReadOnly unfilteredTuple3D) + { + update(unfilteredTuple3D.getX(), unfilteredTuple3D.getY(), unfilteredTuple3D.getZ()); + } + + private final Vector3D unfilteredVector3D = new Vector3D(); + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + if (!hasBeenCalled.getValue()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered, zUnfiltered); + } + else + { + unfilteredVector3D.set(xUnfiltered, yUnfiltered, zUnfiltered); + interpolate(unfilteredVector3D, this, alphaProvider.getValue()); + } + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java new file mode 100644 index 00000000..b5d6d4f9 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java @@ -0,0 +1,89 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoMutableFrameVector3D; +import us.ihmc.yoVariables.filters.ProcessingYoVariable; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class AlphaFilteredYoMutableFrameVector3D extends YoMutableFrameVector3D implements ProcessingYoVariable +{ + private final DoubleProvider alpha; + + private final FrameTuple3DReadOnly unfilteredFrameVector; + private final YoBoolean hasBeenCalled; + + public AlphaFilteredYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, alpha, referenceFrame, null); + } + + public AlphaFilteredYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, + FrameTuple3DReadOnly unfilteredFrameVector) + { + this(namePrefix, nameSuffix, registry, alpha, unfilteredFrameVector.getReferenceFrame(), unfilteredFrameVector); + } + + private AlphaFilteredYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alpha, + ReferenceFrame referenceFrame, FrameTuple3DReadOnly unfilteredFrameVector) + { + super(namePrefix, nameSuffix, registry, referenceFrame); + + this.alpha = alpha; + this.unfilteredFrameVector = unfilteredFrameVector; + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + reset(); + } + + @Override + public void reset() + { + hasBeenCalled.set(false); + } + + @Override + public void update() + { + if (unfilteredFrameVector == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(unfilteredFrameVector); + } + + public void update(FrameTuple3DReadOnly unfilteredFrameTuple) + { + checkReferenceFrameMatch(unfilteredFrameTuple); + update((Tuple3DReadOnly) unfilteredFrameTuple); + } + + public void update(Tuple3DReadOnly unfilteredTuple) + { + update(unfilteredTuple.getX(), unfilteredTuple.getY(), unfilteredTuple.getZ()); + } + + private final Vector3D unfilteredVector3D = new Vector3D(); + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + if (!hasBeenCalled.getValue()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered, zUnfiltered); + } + else + { + unfilteredVector3D.set(xUnfiltered, yUnfiltered, zUnfiltered); + interpolate(unfilteredVector3D, this, alpha.getValue()); + } + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java new file mode 100644 index 00000000..b2d45f92 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java @@ -0,0 +1,71 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.filters.BacklashCompensatingVelocityYoVariable; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; +import us.ihmc.yoVariables.variable.YoDouble; + +public class BacklashCompensatingVelocityYoFrameVector3D extends YoFrameVector3D +{ + private final BacklashCompensatingVelocityYoVariable xDot, yDot, zDot; + + public BacklashCompensatingVelocityYoFrameVector3D(String namePrefix, + String nameSuffix, + YoDouble alpha, + double dt, + YoDouble slopTime, + YoRegistry registry, + YoFramePoint3D yoFramePointToDifferentiate) + { + this(new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), + "", + alpha, + yoFramePointToDifferentiate.getYoX(), + dt, + slopTime, + registry), + new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), + "", + alpha, + yoFramePointToDifferentiate.getYoY(), + dt, + slopTime, + registry), + new BacklashCompensatingVelocityYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), + "", + alpha, + yoFramePointToDifferentiate.getYoZ(), + dt, + slopTime, + registry), + yoFramePointToDifferentiate); + } + + private BacklashCompensatingVelocityYoFrameVector3D(BacklashCompensatingVelocityYoVariable xDot, + BacklashCompensatingVelocityYoVariable yDot, + BacklashCompensatingVelocityYoVariable zDot, + YoFramePoint3D yoFramePointToDifferentiate) + { + super(xDot, yDot, zDot, yoFramePointToDifferentiate.getReferenceFrame()); + + this.xDot = xDot; + this.yDot = yDot; + this.zDot = zDot; + } + + public void update() + { + xDot.update(); + yDot.update(); + zDot.update(); + } + + public void reset() + { + xDot.reset(); + yDot.reset(); + zDot.reset(); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java new file mode 100644 index 00000000..2d49e969 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java @@ -0,0 +1,57 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameTuple3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.filters.BacklashProcessingYoVariable; +import us.ihmc.yoVariables.filters.ProcessingYoVariable; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; + +/** + * This class is designed to perform an estimate of a velocity signal that may contain backlash. It does essentially the same as + * {@link BacklashCompensatingVelocityYoFrameVector3D}, except it takes a velocity signal as input. + * + * It works by zeroing out the estimated velocity whenever the finite-differenced velocity changes sign. It then ramps this value back up to the value returned + * by finite estimating over the course of the slop duration. It assumes that the slop time is some fixed, constant period when the estimate is unreliable. + */ +public class BacklashProcessingYoFrameVector3D extends YoFrameVector3D implements ProcessingYoVariable +{ + private final BacklashProcessingYoVariable xDot, yDot, zDot; + + public BacklashProcessingYoFrameVector3D(String namePrefix, String nameSuffix, double dt, DoubleProvider slopTime, + YoRegistry registry, YoFrameTuple3D yoFrameTupleToProcess) + { + this(new BacklashProcessingYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), "", yoFrameTupleToProcess.getYoX(), dt, slopTime, registry), + new BacklashProcessingYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), "", yoFrameTupleToProcess.getYoY(), dt, slopTime, registry), + new BacklashProcessingYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), "", yoFrameTupleToProcess.getYoZ(), dt, slopTime, registry), + yoFrameTupleToProcess.getReferenceFrame()); + } + + private BacklashProcessingYoFrameVector3D(BacklashProcessingYoVariable xDot, BacklashProcessingYoVariable yDot, BacklashProcessingYoVariable zDot, + ReferenceFrame referenceFrame) + { + super(xDot, yDot, zDot, referenceFrame); + + this.xDot = xDot; + this.yDot = yDot; + this.zDot = zDot; + } + + @Override + public void update() + { + xDot.update(); + yDot.update(); + zDot.update(); + } + + @Override + public void reset() + { + xDot.reset(); + yDot.reset(); + zDot.reset(); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java new file mode 100644 index 00000000..4c064393 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java @@ -0,0 +1,57 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.filters.DeadbandedYoVariable; +import us.ihmc.yoVariables.filters.ProcessingYoVariable; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; + +public class DeadbandedYoFrameVector3D extends YoFrameVector3D implements ProcessingYoVariable +{ + private final DeadbandedYoVariable x, y, z; + + public DeadbandedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider deadzoneSize, YoFrameVector3D tupleToTrack) + { + this(new DeadbandedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), tupleToTrack.getYoX(), deadzoneSize, registry), + new DeadbandedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), tupleToTrack.getYoY(), deadzoneSize, registry), + new DeadbandedYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), tupleToTrack.getYoZ(), deadzoneSize, registry), + tupleToTrack.getReferenceFrame()); + } + + public DeadbandedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider deadzoneSize, ReferenceFrame referenceFrame) + { + this(new DeadbandedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), deadzoneSize, registry), + new DeadbandedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), deadzoneSize, registry), + new DeadbandedYoVariable(YoGeometryNameTools.createZName(namePrefix, nameSuffix), deadzoneSize, registry), + referenceFrame); + } + + private DeadbandedYoFrameVector3D(DeadbandedYoVariable x, DeadbandedYoVariable y, DeadbandedYoVariable z, ReferenceFrame referenceFrame) + { + super(x, y, z, referenceFrame); + + this.x = x; + this.y = y; + this.z = z; + } + + @Override + public void update() + { + x.update(); + y.update(); + z.update(); + } + + public void update(FrameTuple3DReadOnly frameTuple) + { + checkReferenceFrameMatch(frameTuple); + + x.update(frameTuple.getX()); + y.update(frameTuple.getY()); + z.update(frameTuple.getZ()); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java new file mode 100644 index 00000000..c43f4327 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java @@ -0,0 +1,113 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; +import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +/** + *

+ * {@link FilteredFiniteDifferenceYoFrameVector2D} + *

+ * + *

+ * Differentiates and Filters a {@link YoFrameVector2D} to get its derivative. This derviative is then low pass filtered. + *

+ * 
+ *            vel_{n} = alpha * vel{n-1} + (1 - alpha) * (pos_{n} - pos_{n-1})
+ *
+ */ +public class FilteredFiniteDifferenceYoFrameVector2D extends YoFrameVector2D +{ + private final double dt; + private final DoubleProvider alphaProvider; + + private final YoBoolean hasBeenCalled; + private final FrameTuple2DReadOnly currentPosition; + private final YoFrameVector2D lastPosition; + + public FilteredFiniteDifferenceYoFrameVector2D(String namePrefix, + String nameSuffix, + DoubleProvider alpha, + double dt, + YoRegistry registry, + FrameTuple2DReadOnly frameTuple2DToDifferentiate) + { + this(namePrefix, nameSuffix, alpha, dt, registry, frameTuple2DToDifferentiate, frameTuple2DToDifferentiate.getReferenceFrame()); + } + + public FilteredFiniteDifferenceYoFrameVector2D(String namePrefix, + String nameSuffix, + DoubleProvider alpha, + double dt, + YoRegistry registry, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, alpha, dt, registry, null, referenceFrame); + } + + private FilteredFiniteDifferenceYoFrameVector2D(String namePrefix, + String nameSuffix, + DoubleProvider alpha, + double dt, + YoRegistry registry, + FrameTuple2DReadOnly frameTuple2DToDifferentiate, + ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + this.alphaProvider = alpha; + this.dt = dt; + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + currentPosition = frameTuple2DToDifferentiate; + lastPosition = new YoFrameVector2D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (currentPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(FrameTuple3DReadOnly)"); + } + + update(currentPosition); + } + + public void update(FrameTuple2DReadOnly frameTuple) + { + checkReferenceFrameMatch(frameTuple); + update((Tuple2DReadOnly) frameTuple); + } + + private final Vector2D currentRawDerivative = new Vector2D(); + + public void update(Tuple2DReadOnly currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + lastPosition.set(currentPosition); + setToZero(); + } + + currentRawDerivative.sub(currentPosition, lastPosition); + currentRawDerivative.scale(1.0 / dt); + + interpolate(currentRawDerivative, this, alphaProvider.getValue()); + + lastPosition.set(currentPosition); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java new file mode 100644 index 00000000..557eff95 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java @@ -0,0 +1,113 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +/** + *

+ * {@link FilteredFiniteDifferenceYoFrameVector3D} + *

+ * + *

+ * Differentiates and Filters a {@link YoFrameVector3D} to get its derivative. This derviative is then low pass filtered. + *

+ * 
+ *            vel_{n} = alpha * vel{n-1} + (1 - alpha) * (pos_{n} - pos_{n-1})
+ *
+ */ +public class FilteredFiniteDifferenceYoFrameVector3D extends YoFrameVector3D +{ + private final double dt; + private final DoubleProvider alphaProvider; + + private final YoBoolean hasBeenCalled; + private final FrameTuple3DReadOnly currentPosition; + private final YoFrameVector3D lastPosition; + + public FilteredFiniteDifferenceYoFrameVector3D(String namePrefix, + String nameSuffix, + DoubleProvider alpha, + double dt, + YoRegistry registry, + FrameTuple3DReadOnly frameTuple3DToDifferentiate) + { + this(namePrefix, nameSuffix, alpha, dt, registry, frameTuple3DToDifferentiate, frameTuple3DToDifferentiate.getReferenceFrame()); + } + + public FilteredFiniteDifferenceYoFrameVector3D(String namePrefix, + String nameSuffix, + DoubleProvider alpha, + double dt, + YoRegistry registry, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, alpha, dt, registry, null, referenceFrame); + } + + private FilteredFiniteDifferenceYoFrameVector3D(String namePrefix, + String nameSuffix, + DoubleProvider alpha, + double dt, + YoRegistry registry, + FrameTuple3DReadOnly frameTuple3DToDifferentiate, + ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + this.alphaProvider = alpha; + this.dt = dt; + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + currentPosition = frameTuple3DToDifferentiate; + lastPosition = new YoFrameVector3D(namePrefix + "_lastPosition", nameSuffix, getReferenceFrame(), registry); + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (currentPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(FrameTuple3DReadOnly)"); + } + + update(currentPosition); + } + + public void update(FrameTuple3DReadOnly frameTuple) + { + checkReferenceFrameMatch(frameTuple); + update((Tuple3DReadOnly) frameTuple); + } + + private final Vector3D currentRawDerivative = new Vector3D(); + + public void update(Tuple3DReadOnly currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + lastPosition.set(currentPosition); + setToZero(); + } + + currentRawDerivative.sub(currentPosition, lastPosition); + currentRawDerivative.scale(1.0 / dt); + + interpolate(currentRawDerivative, this, alphaProvider.getValue()); + + lastPosition.set(currentPosition); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java new file mode 100644 index 00000000..1576414a --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java @@ -0,0 +1,98 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.matrix.RotationMatrix; +import us.ihmc.euclid.matrix.interfaces.RotationMatrixReadOnly; +import us.ihmc.euclid.orientation.interfaces.Orientation3DReadOnly; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class FiniteDifferenceAngularVelocityYoFrameVector3D extends YoFrameVector3D +{ + private final YoFrameQuaternion orientation; + private final YoFrameQuaternion orientationPreviousValue; + + private final YoBoolean hasBeenCalled; + + private final RotationMatrix currentOrientationMatrix = new RotationMatrix(); + private final RotationMatrix previousOrientationMatrix = new RotationMatrix(); + private final RotationMatrix deltaOrientationMatrix = new RotationMatrix(); + private final AxisAngle deltaAxisAngle = new AxisAngle(); + + private final double dt; + + public FiniteDifferenceAngularVelocityYoFrameVector3D(String namePrefix, ReferenceFrame referenceFrame, double dt, YoRegistry registry) + { + this(namePrefix, null, referenceFrame, dt, registry); + } + + public FiniteDifferenceAngularVelocityYoFrameVector3D(String namePrefix, YoFrameQuaternion orientationToDifferentiate, double dt, YoRegistry registry) + { + this(namePrefix, orientationToDifferentiate, orientationToDifferentiate.getReferenceFrame(), dt, registry); + } + + private FiniteDifferenceAngularVelocityYoFrameVector3D(String namePrefix, YoFrameQuaternion orientationToDifferentiate, ReferenceFrame referenceFrame, double dt, YoRegistry registry) + { + super(namePrefix, referenceFrame, registry); + + this.dt = dt; + + orientation = orientationToDifferentiate; + orientationPreviousValue = new YoFrameQuaternion(namePrefix + "_previous", referenceFrame, registry); + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, "", registry); + hasBeenCalled.set(false); + } + + public void update() + { + if (orientation == null) + { + throw new NullPointerException("FiniteDifferenceAngularVelocityYoFrameVector must be constructed with a non null " + + "orientation variable to call update(), otherwise use update(FrameOrientation)"); + } + + currentOrientationMatrix.set(orientation); + update(currentOrientationMatrix); + } + + public void update(FrameOrientation3DReadOnly currentOrientation) + { + checkReferenceFrameMatch(currentOrientation); + + currentOrientationMatrix.set(currentOrientation); + update(currentOrientationMatrix); + } + + public void update(Orientation3DReadOnly currentOrientation) + { + currentOrientationMatrix.set(currentOrientation); + update(currentOrientationMatrix); + } + + public void update(RotationMatrixReadOnly rotationMatrix) + { + if (!hasBeenCalled.getBooleanValue()) + { + orientationPreviousValue.set(rotationMatrix); + hasBeenCalled.set(true); + } + + if (rotationMatrix != currentOrientationMatrix) + currentOrientationMatrix.set(rotationMatrix); + previousOrientationMatrix.set(orientationPreviousValue); + deltaOrientationMatrix.set(currentOrientationMatrix); + deltaOrientationMatrix.multiplyTransposeOther(previousOrientationMatrix); + deltaAxisAngle.set(deltaOrientationMatrix); + + set(deltaAxisAngle.getX(), deltaAxisAngle.getY(), deltaAxisAngle.getZ()); + scale(deltaAxisAngle.getAngle() / dt); + + orientationPreviousValue.set(currentOrientationMatrix); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java new file mode 100644 index 00000000..e06c568e --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java @@ -0,0 +1,65 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; +import us.ihmc.yoVariables.filters.MovingAverageYoDouble; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; + +public class MovingAverageYoFramePoint2D extends YoFramePoint2D +{ + private final MovingAverageYoDouble x, y; + + public MovingAverageYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, int beta, ReferenceFrame referenceFrame) + { + this(new MovingAverageYoDouble(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta), + new MovingAverageYoDouble(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta), + referenceFrame); + } + + public MovingAverageYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, int beta, YoFramePoint2D unfilteredPoint) + { + this(new MovingAverageYoDouble(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoX()), + new MovingAverageYoDouble(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoY()), + unfilteredPoint.getReferenceFrame()); + } + + private MovingAverageYoFramePoint2D(MovingAverageYoDouble x, MovingAverageYoDouble y, ReferenceFrame referenceFrame) + { + super(x, y, referenceFrame); + + this.x = x; + this.y = y; + } + + public void update() + { + x.update(); + y.update(); + } + + public void update(double xUnfiltered, double yUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + } + + public void update(Point2DReadOnly point2dUnfiltered) + { + update(point2dUnfiltered.getX(), point2dUnfiltered.getY()); + } + + public void update(FramePoint2DReadOnly point2dUnfiltered) + { + checkReferenceFrameMatch(point2dUnfiltered); + update((Point2DReadOnly) point2dUnfiltered); + } + + public void reset() + { + x.reset(); + y.reset(); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java new file mode 100644 index 00000000..cebccec1 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java @@ -0,0 +1,65 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; +import us.ihmc.yoVariables.filters.MovingAverageYoDouble; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; + +public class MovingAverageYoFrameVector2D extends YoFrameVector2D +{ + private final MovingAverageYoDouble x, y; + + public MovingAverageYoFrameVector2D(String namePrefix, String nameSuffix, YoRegistry registry, int beta, ReferenceFrame referenceFrame) + { + this(new MovingAverageYoDouble(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta), + new MovingAverageYoDouble(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta), + referenceFrame); + } + + public MovingAverageYoFrameVector2D(String namePrefix, String nameSuffix, YoRegistry registry, int beta, YoFrameVector2D unfilteredVector) + { + this(new MovingAverageYoDouble(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoX()), + new MovingAverageYoDouble(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredVector.getYoY()), + unfilteredVector.getReferenceFrame()); + } + + private MovingAverageYoFrameVector2D(MovingAverageYoDouble x, MovingAverageYoDouble y, ReferenceFrame referenceFrame) + { + super(x, y, referenceFrame); + + this.x = x; + this.y = y; + } + + public void update() + { + x.update(); + y.update(); + } + + public void update(double xUnfiltered, double yUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + } + + public void update(Vector2DReadOnly vector2dUnfiltered) + { + update(vector2dUnfiltered.getX(), vector2dUnfiltered.getY()); + } + + public void update(FrameVector2DReadOnly vector2dUnfiltered) + { + checkReferenceFrameMatch(vector2dUnfiltered); + update((Vector2DReadOnly) vector2dUnfiltered); + } + + public void reset() + { + x.reset(); + y.reset(); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java new file mode 100644 index 00000000..fef4624a --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java @@ -0,0 +1,145 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.FrameQuaternion; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameYawPitchRoll; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class RateLimitedYoFrameOrientation extends YoFrameYawPitchRoll +{ + private final DoubleProvider maxRateVariable; + + private final YoFrameYawPitchRoll rawOrientation; + private final YoBoolean limited; + private final YoBoolean hasBeenCalled; + private final double dt; + + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + YoFrameYawPitchRoll rawOrientation) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawOrientation, rawOrientation.getReferenceFrame()); + } + + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); + } + + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, + YoFrameYawPitchRoll rawOrientation) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawOrientation, + rawOrientation.getReferenceFrame()); + } + + public RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); + } + + private RateLimitedYoFrameOrientation(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + YoFrameYawPitchRoll rawOrientation, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + limited = VariableTools.createLimitedCalledYoBoolean(namePrefix, nameSuffix, registry); + + if (maxRate == null) + maxRate = VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; + + this.rawOrientation = rawOrientation; + + this.dt = dt; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawOrientation == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawOrientation); + } + + public void update(YoFrameYawPitchRoll yoFrameVectorUnfiltered) + { + checkReferenceFrameMatch(yoFrameVectorUnfiltered); + update(yoFrameVectorUnfiltered.getYaw(), yoFrameVectorUnfiltered.getPitch(), yoFrameVectorUnfiltered.getRoll()); + } + + public void update(FrameQuaternion frameOrientationUnfiltered) + { + checkReferenceFrameMatch(frameOrientationUnfiltered); + update((QuaternionReadOnly) frameOrientationUnfiltered); + } + + private final Quaternion quaternionUnfiltered = new Quaternion(); + + public void update(double yawUnfiltered, double pitchUnfiltered, double rollUnfiltered) + { + quaternionUnfiltered.setYawPitchRoll(yawUnfiltered, pitchUnfiltered, rollUnfiltered); + update(quaternionUnfiltered); + } + + private final Quaternion quaternionFiltered = new Quaternion(); + private final Quaternion difference = new Quaternion(); + private final Vector3D limitedRotationVector = new Vector3D(); + + public void update(QuaternionReadOnly quaternionUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + limited.set(false); + set(quaternionUnfiltered); + return; + } + + quaternionFiltered.set(this); + + if (quaternionFiltered.dot(quaternionUnfiltered) > 0.0) + { + difference.difference(quaternionFiltered, quaternionUnfiltered); + } + else + { + difference.setAndNegate(quaternionUnfiltered); + difference.preMultiplyConjugateOther(quaternionFiltered); + } + + difference.getRotationVector(limitedRotationVector); + boolean clipped = limitedRotationVector.clipToMaxNorm(dt * maxRateVariable.getValue()); + limited.set(clipped); + + if (clipped) + { + difference.setRotationVector(limitedRotationVector); + quaternionFiltered.multiply(difference); + set(quaternionFiltered); + } + else + { + set(quaternionUnfiltered); + } + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java new file mode 100644 index 00000000..0a5575d1 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java @@ -0,0 +1,135 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector2D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint2D; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class RateLimitedYoFramePoint2D extends YoFramePoint2D +{ + private final DoubleProvider maxRateVariable; + + private final FrameTuple2DReadOnly rawPosition; + private final YoBoolean limited; + private final YoBoolean hasBeenCalled; + private final double dt; + + private final FrameVector2D differenceVector = new FrameVector2D(); + + private static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } + + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple2DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); + } + + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); + } + + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, + FrameTuple2DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, + rawPosition.getReferenceFrame()); + } + + public RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); + } + + private RateLimitedYoFramePoint2D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple2DReadOnly rawPosition, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + limited = VariableTools.createLimitedCalledYoBoolean(namePrefix, nameSuffix, registry); + + if (maxRate == null) + maxRate = createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; + + this.rawPosition = rawPosition; + + this.dt = dt; + + reset(); + } + + public void setAndUpdate(FramePoint2DReadOnly framePoint2D) + { + super.set(framePoint2D); + hasBeenCalled.set(true); + } + + public void setAndUpdate(FramePoint3DReadOnly framePoint3D) + { + super.set(framePoint3D); + hasBeenCalled.set(true); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawPosition); + } + + public void update(FrameTuple2DReadOnly frameVectorUnfiltered) + { + checkReferenceFrameMatch(frameVectorUnfiltered); + update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY()); + } + + public void update(Tuple2DReadOnly vectorUnfiltered) + { + update(vectorUnfiltered.getX(), vectorUnfiltered.getY()); + } + + public void update(double xUnfiltered, double yUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered); + } + + if (maxRateVariable.getValue() < 0) + throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); + + differenceVector.setToZero(getReferenceFrame()); + differenceVector.set(xUnfiltered, yUnfiltered); + differenceVector.sub(getX(), getY()); + + limited.set(differenceVector.clipToMaxNorm(maxRateVariable.getValue() * dt)); + add(differenceVector); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java new file mode 100644 index 00000000..5b7ad253 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java @@ -0,0 +1,113 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFramePoint3D; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class RateLimitedYoFramePoint3D extends YoFramePoint3D +{ + private final DoubleProvider maxRateVariable; + + private final FrameTuple3DReadOnly rawPosition; + private final YoBoolean limited; + private final YoBoolean hasBeenCalled; + private final double dt; + + private final FrameVector3D differenceVector = new FrameVector3D(); + + public RateLimitedYoFramePoint3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); + } + + public RateLimitedYoFramePoint3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); + } + + public RateLimitedYoFramePoint3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, + FrameTuple3DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, + rawPosition.getReferenceFrame()); + } + + public RateLimitedYoFramePoint3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); + } + + private RateLimitedYoFramePoint3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + limited = VariableTools.createLimitedCalledYoBoolean(namePrefix, nameSuffix, registry); + + if (maxRate == null) + maxRate = VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; + + this.rawPosition = rawPosition; + + this.dt = dt; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawPosition); + } + + public void update(FrameTuple3DReadOnly frameVectorUnfiltered) + { + checkReferenceFrameMatch(frameVectorUnfiltered); + update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); + } + + public void update(Tuple3DReadOnly vectorUnfiltered) + { + update(vectorUnfiltered.getX(), vectorUnfiltered.getY(), vectorUnfiltered.getZ()); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered, zUnfiltered); + } + + if (maxRateVariable.getValue() < 0) + throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); + + differenceVector.setToZero(getReferenceFrame()); + differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); + differenceVector.sub(getX(), getY(), getZ()); + + limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); + add(differenceVector); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java new file mode 100644 index 00000000..f27cdf31 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java @@ -0,0 +1,103 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.geometry.tools.EuclidGeometryIOTools; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePoint3DBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePose3DBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameQuaternionBasics; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; + +public class RateLimitedYoFramePose3D implements FixedFramePose3DBasics +{ + private final RateLimitedYoFramePoint3D position; + private final RateLimitedYoFrameQuaternion orientation; + + public RateLimitedYoFramePose3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + FramePose3DReadOnly rawPose) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawPose, rawPose.getReferenceFrame()); + } + + public RateLimitedYoFramePose3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); + } + + public RateLimitedYoFramePose3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, FramePose3DReadOnly rawPose) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPose, rawPose.getReferenceFrame()); + } + + public RateLimitedYoFramePose3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); + } + + private RateLimitedYoFramePose3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + FramePose3DReadOnly rawPose, ReferenceFrame referenceFrame) + { + if (rawPose != null) + { + this.position = new RateLimitedYoFramePoint3D(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, rawPose.getPosition()); + this.orientation = new RateLimitedYoFrameQuaternion(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, rawPose.getOrientation()); + } + else + { + this.position = new RateLimitedYoFramePoint3D(namePrefix, "Position" + nameSuffix, registry, maxRate, dt, referenceFrame); + this.orientation = new RateLimitedYoFrameQuaternion(namePrefix, "Orientation" + nameSuffix, registry, maxRate, dt, referenceFrame); + } + + reset(); + } + + public void reset() + { + position.reset(); + orientation.reset(); + } + + public void update() + { + position.update(); + orientation.update(); + + set(position, orientation); + } + + public void update(FramePose3DReadOnly framePoseUnfiltered) + { + checkReferenceFrameMatch(framePoseUnfiltered); + position.update(framePoseUnfiltered.getPosition()); + orientation.update(framePoseUnfiltered.getOrientation()); + + set(position, orientation); + } + + @Override + public FixedFramePoint3DBasics getPosition() + { + return position; + } + + @Override + public FixedFrameQuaternionBasics getOrientation() + { + return orientation; + } + + @Override + public ReferenceFrame getReferenceFrame() + { + return position.getReferenceFrame(); + } + + @Override + public String toString() + { + return EuclidGeometryIOTools.getPose3DString(this) + "-" + getReferenceFrame(); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java new file mode 100644 index 00000000..7cbcebea --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java @@ -0,0 +1,144 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameOrientation3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameQuaternionReadOnly; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class RateLimitedYoFrameQuaternion extends YoFrameQuaternion +{ + private final QuaternionReadOnly rawQuaternion; + private final YoBoolean hasBeenCalled; + private final double dt; + + private final YoBoolean limited; + private final DoubleProvider maxRateVariable; + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, + FrameQuaternionReadOnly rawQuaternion) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawQuaternion.getReferenceFrame(), + rawQuaternion); + } + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, referenceFrame, null); + } + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, + ReferenceFrame referenceFrame, QuaternionReadOnly rawQuaternion) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, referenceFrame, rawQuaternion); + } + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + FrameQuaternionReadOnly rawQuaternion) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawQuaternion.getReferenceFrame(), rawQuaternion); + } + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, referenceFrame, null); + } + + public RateLimitedYoFrameQuaternion(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame, QuaternionReadOnly rawQuaternion) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + limited = VariableTools.createLimitedCalledYoBoolean(namePrefix, nameSuffix, registry); + + if (maxRate == null) + maxRate = VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; + + this.rawQuaternion = rawQuaternion; + + this.dt = dt; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawQuaternion == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawQuaternion); + } + + private final Quaternion quaternion = new Quaternion(); + + public void update(FrameOrientation3DReadOnly frameOrientationUnfiltered) + { + checkReferenceFrameMatch(frameOrientationUnfiltered); + quaternion.set(frameOrientationUnfiltered); + update(quaternion); + } + + public void update(FrameQuaternionReadOnly frameQuaternionUnfiltered) + { + checkReferenceFrameMatch(frameQuaternionUnfiltered); + quaternion.set(frameQuaternionUnfiltered); + update(quaternion); + } + + private final Quaternion difference = new Quaternion(); + private final Vector3D limitedRotationVector = new Vector3D(); + + public void update(QuaternionReadOnly quaternionUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + limited.set(false); + set(quaternionUnfiltered); + return; + } + + if (dot(quaternionUnfiltered) > 0.0) + { + difference.difference(this, quaternionUnfiltered); + } + else + { + difference.setAndNegate(quaternionUnfiltered); + difference.preMultiplyConjugateOther(this); + } + + difference.getRotationVector(limitedRotationVector); + boolean clipped = limitedRotationVector.clipToMaxNorm(dt * maxRateVariable.getValue()); + limited.set(clipped); + + if (clipped) + { + difference.setRotationVector(limitedRotationVector); + multiply(difference); + } + else + { + set(quaternionUnfiltered); + } + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java new file mode 100644 index 00000000..da92c1d8 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java @@ -0,0 +1,48 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector2D; +import us.ihmc.yoVariables.filters.RateLimitedYoVariable; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; + +public class RateLimitedYoFrameVector2D extends YoFrameVector2D +{ + private final RateLimitedYoVariable x, y; + + public RateLimitedYoFrameVector2D(String namePrefix, YoRegistry registry, + DoubleProvider maxRate, double dt, YoFrameVector2D unfilteredVector) + { + this(namePrefix, "", registry, maxRate, dt, unfilteredVector); + } + + public RateLimitedYoFrameVector2D(String namePrefix, String nameSuffix, YoRegistry registry, + DoubleProvider maxRate, double dt, YoFrameVector2D unfilteredVector) + { + this(new RateLimitedYoVariable(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt), + new RateLimitedYoVariable(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt), + unfilteredVector.getReferenceFrame()); + } + + private RateLimitedYoFrameVector2D(RateLimitedYoVariable x, RateLimitedYoVariable y, ReferenceFrame referenceFrame) + { + super(x, y, referenceFrame); + + this.x = x; + this.y = y; + } + + public void update() + { + x.update(); + y.update(); + } + + public void reset() + { + x.reset(); + y.reset(); + } +} + diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java new file mode 100644 index 00000000..64760b2e --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java @@ -0,0 +1,114 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class RateLimitedYoFrameVector3D extends YoFrameVector3D +{ + private final DoubleProvider maxRateVariable; + + private final FrameTuple3DReadOnly rawPosition; + private final YoBoolean limited; + private final YoBoolean hasBeenCalled; + private final double dt; + + private final FrameVector3D differenceVector = new FrameVector3D(); + + + public RateLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); + } + + public RateLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, null, referenceFrame); + } + + public RateLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, + FrameTuple3DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, rawPosition, + rawPosition.getReferenceFrame()); + } + + public RateLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) + { + this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry), dt, null, referenceFrame); + } + + private RateLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + limited = VariableTools.createLimitedCalledYoBoolean(namePrefix, nameSuffix, registry); + + if (maxRate == null) + maxRate = VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; + + this.rawPosition = rawPosition; + + this.dt = dt; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawPosition); + } + + public void update(FrameTuple3DReadOnly frameVectorUnfiltered) + { + checkReferenceFrameMatch(frameVectorUnfiltered); + update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); + } + + public void update(Tuple3DReadOnly vectorUnfiltered) + { + update(vectorUnfiltered.getX(), vectorUnfiltered.getY(), vectorUnfiltered.getZ()); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered, zUnfiltered); + } + + if (maxRateVariable.getValue() < 0) + throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); + + differenceVector.setToZero(getReferenceFrame()); + differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); + differenceVector.sub(getX(), getY(), getZ()); + + limited.set(differenceVector.clipToMaxNorm(maxRateVariable.getValue() * dt)); + add(differenceVector); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java new file mode 100644 index 00000000..8e68b73a --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java @@ -0,0 +1,95 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoMutableFrameVector3D; +import us.ihmc.yoVariables.filters.VariableTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +public class RateLimitedYoMutableFrameVector3D extends YoMutableFrameVector3D +{ + private final DoubleProvider maxRateVariable; + + private final FrameTuple3DReadOnly rawPosition; + private final YoBoolean limited; + private final YoBoolean hasBeenCalled; + private final double dt; + + private final FrameVector3D differenceVector = new FrameVector3D(); + + public RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition) + { + this(namePrefix, nameSuffix, registry, maxRate, dt, rawPosition, rawPosition.getReferenceFrame()); + } + + private RateLimitedYoMutableFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate, double dt, + FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame) + { + super(namePrefix, nameSuffix, registry, referenceFrame); + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(namePrefix, nameSuffix, registry); + limited = VariableTools.createLimitedCalledYoBoolean(namePrefix, nameSuffix, registry); + + if (maxRate == null) + maxRate = VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry); + + maxRateVariable = maxRate; + + this.rawPosition = rawPosition; + + this.dt = dt; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (rawPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(rawPosition); + } + + public void update(FrameTuple3DReadOnly frameVectorUnfiltered) + { + checkReferenceFrameMatch(frameVectorUnfiltered); + update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ()); + } + + public void update(Tuple3DReadOnly vectorUnfiltered) + { + update(vectorUnfiltered.getX(), vectorUnfiltered.getY(), vectorUnfiltered.getZ()); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + if (!hasBeenCalled.getBooleanValue() || containsNaN()) + { + hasBeenCalled.set(true); + set(xUnfiltered, yUnfiltered, zUnfiltered); + } + + if (maxRateVariable.getValue() < 0) + throw new RuntimeException("The maxRate parameter in the " + getClass().getSimpleName() + " cannot be negative."); + + differenceVector.setToZero(getReferenceFrame()); + differenceVector.set(xUnfiltered, yUnfiltered, zUnfiltered); + differenceVector.sub(getX(), getY(), getZ()); + + limited.set(differenceVector.clipToMaxLength(maxRateVariable.getValue() * dt)); + add(differenceVector); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java new file mode 100644 index 00000000..556f6bd5 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java @@ -0,0 +1,78 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; +import us.ihmc.yoVariables.variable.YoInteger; + +public class RunningAverageYoFrameVector3D extends YoFrameVector3D +{ + private final YoInteger sampleSize; + private final Tuple3DReadOnly dataSource; + + public RunningAverageYoFrameVector3D(String namePrefix, ReferenceFrame referenceFrame, YoRegistry registry) + { + this(namePrefix, "", registry, referenceFrame, null); + } + + public RunningAverageYoFrameVector3D(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, YoRegistry registry) + { + this(namePrefix, nameSuffix, registry, referenceFrame, null); + } + + public RunningAverageYoFrameVector3D(String namePrefix, YoRegistry registry, FrameTuple3DReadOnly dataSource) + { + this(namePrefix, "", registry, dataSource.getReferenceFrame(), dataSource); + } + + public RunningAverageYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, FrameTuple3DReadOnly dataSource) + { + this(namePrefix, nameSuffix, registry, dataSource.getReferenceFrame(), dataSource); + } + + private RunningAverageYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, ReferenceFrame referenceFrame, Tuple3DReadOnly dataSource) + { + super(namePrefix, nameSuffix, referenceFrame, registry); + + this.dataSource = dataSource; + + sampleSize = new YoInteger(YoGeometryNameTools.assembleName(namePrefix, "sampleSize", nameSuffix), registry); + } + + public void update() + { + update(dataSource); + } + + public void update(Tuple3DReadOnly vectorSource) + { + update(vectorSource.getX(), vectorSource.getY(), vectorSource.getZ()); + } + + public void update(FrameTuple3DReadOnly vectorSource) + { + checkReferenceFrameMatch(vectorSource); + update((Tuple3DReadOnly) vectorSource); + } + + public void update(double xSource, double ySource, double zSource) + { + sampleSize.increment(); + addX((xSource - getX()) / sampleSize.getValue()); + addY((ySource - getY()) / sampleSize.getValue()); + addZ((zSource - getZ()) / sampleSize.getValue()); + } + + public void reset() + { + sampleSize.set(0); + } + + public int getSampleSize() + { + return sampleSize.getValue(); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java new file mode 100644 index 00000000..57f6d78c --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java @@ -0,0 +1,129 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.filters.ProcessingYoVariable; +import us.ihmc.yoVariables.filters.SecondOrderFilterType; +import us.ihmc.yoVariables.filters.SecondOrderFilteredYoDouble; +import us.ihmc.yoVariables.filters.SecondOrderFilteredYoVariableParameters; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; + +public class SecondOrderFilteredYoFrameVector3D extends YoFrameVector3D implements ProcessingYoVariable +{ + private final SecondOrderFilteredYoDouble x, y, z; + + public SecondOrderFilteredYoFrameVector3D(String namePrefix, + String nameSuffix, + YoRegistry registry, + double dt, + SecondOrderFilteredYoVariableParameters parameters, + YoFrameVector3D unfilteredVector) + { + this(new SecondOrderFilteredYoDouble(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoX()), + new SecondOrderFilteredYoDouble(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoY()), + new SecondOrderFilteredYoDouble(YoGeometryNameTools.createZName(namePrefix, nameSuffix), registry, dt, parameters, unfilteredVector.getYoZ()), + unfilteredVector.getReferenceFrame()); + } + + private SecondOrderFilteredYoFrameVector3D(SecondOrderFilteredYoDouble x, + SecondOrderFilteredYoDouble y, + SecondOrderFilteredYoDouble z, + ReferenceFrame referenceFrame) + { + super(x, y, z, referenceFrame); + + this.x = x; + this.y = y; + this.z = z; + } + + public static SecondOrderFilteredYoFrameVector3D createSecondOrderFilteredYoFrameVector(String namePrefix, + String nameSuffix, + YoRegistry registry, + double dt, + double naturalFrequencyInHz, + double dampingRatio, + SecondOrderFilterType filterType, + ReferenceFrame referenceFrame) + + { + SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, + registry, + naturalFrequencyInHz, + dampingRatio, + filterType); + return createSecondOrderFilteredYoFrameVector(namePrefix, nameSuffix, registry, dt, parameters, referenceFrame); + } + + public static SecondOrderFilteredYoFrameVector3D createSecondOrderFilteredYoFrameVector(String namePrefix, + String nameSuffix, + YoRegistry registry, + double dt, + SecondOrderFilteredYoVariableParameters parameters, + ReferenceFrame referenceFrame) + { + SecondOrderFilteredYoDouble x, y, z; + x = new SecondOrderFilteredYoDouble(YoGeometryNameTools.createXName(namePrefix, nameSuffix), registry, dt, parameters); + y = new SecondOrderFilteredYoDouble(YoGeometryNameTools.createYName(namePrefix, nameSuffix), registry, dt, parameters); + z = new SecondOrderFilteredYoDouble(YoGeometryNameTools.createZName(namePrefix, nameSuffix), registry, dt, parameters); + return new SecondOrderFilteredYoFrameVector3D(x, y, z, referenceFrame); + } + + public static SecondOrderFilteredYoFrameVector3D createSecondOrderFilteredYoFrameVector(String namePrefix, + String nameSuffix, + YoRegistry registry, + double dt, + double naturalFrequencyInHz, + double dampingRatio, + SecondOrderFilterType filterType, + YoFrameVector3D unfilteredVector) + { + SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, + registry, + naturalFrequencyInHz, + dampingRatio, + filterType); + return new SecondOrderFilteredYoFrameVector3D(namePrefix, nameSuffix, registry, dt, parameters, unfilteredVector); + } + + @Override + public void update() + { + x.update(); + y.update(); + z.update(); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + z.update(zUnfiltered); + } + + public void update(Vector3D vectorUnfiltered) + { + x.update(vectorUnfiltered.getX()); + y.update(vectorUnfiltered.getY()); + z.update(vectorUnfiltered.getZ()); + } + + public void update(FrameVector3D vectorUnfiltered) + { + checkReferenceFrameMatch(vectorUnfiltered); + x.update(vectorUnfiltered.getX()); + y.update(vectorUnfiltered.getY()); + z.update(vectorUnfiltered.getZ()); + } + + @Override + public void reset() + { + x.reset(); + y.reset(); + z.reset(); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java new file mode 100644 index 00000000..07e32c51 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java @@ -0,0 +1,96 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; +import us.ihmc.yoVariables.filters.ProcessingYoVariable; +import us.ihmc.yoVariables.filters.SimpleMovingAverageFilteredYoVariable; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; + +public class SimpleMovingAverageFilteredYoFrameVector3D extends YoFrameVector3D implements ProcessingYoVariable +{ + private final SimpleMovingAverageFilteredYoVariable x, y, z; + + private SimpleMovingAverageFilteredYoFrameVector3D(SimpleMovingAverageFilteredYoVariable x, SimpleMovingAverageFilteredYoVariable y, + SimpleMovingAverageFilteredYoVariable z, ReferenceFrame referenceFrame) + { + super(x, y, z, referenceFrame); + + this.x = x; + this.y = y; + this.z = z; + } + + public static SimpleMovingAverageFilteredYoFrameVector3D createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, + ReferenceFrame referenceFrame, YoRegistry registry) + { + String xName = YoGeometryNameTools.createXName(namePrefix, nameSuffix); + String yName = YoGeometryNameTools.createYName(namePrefix, nameSuffix); + String zName = YoGeometryNameTools.createZName(namePrefix, nameSuffix); + + SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, registry); + SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, registry); + SimpleMovingAverageFilteredYoVariable z = new SimpleMovingAverageFilteredYoVariable(zName, windowSize, registry); + + return new SimpleMovingAverageFilteredYoFrameVector3D(x, y, z, referenceFrame); + } + + public static SimpleMovingAverageFilteredYoFrameVector3D createSimpleMovingAverageFilteredYoFrameVector(String namePrefix, String nameSuffix, int windowSize, + YoFrameVector3D unfilteredVector, YoRegistry registry) + { + String xName = YoGeometryNameTools.createXName(namePrefix, nameSuffix); + String yName = YoGeometryNameTools.createYName(namePrefix, nameSuffix); + String zName = YoGeometryNameTools.createZName(namePrefix, nameSuffix); + + SimpleMovingAverageFilteredYoVariable x = new SimpleMovingAverageFilteredYoVariable(xName, windowSize, unfilteredVector.getYoX(), registry); + SimpleMovingAverageFilteredYoVariable y = new SimpleMovingAverageFilteredYoVariable(yName, windowSize, unfilteredVector.getYoY(), registry); + SimpleMovingAverageFilteredYoVariable z = new SimpleMovingAverageFilteredYoVariable(zName, windowSize, unfilteredVector.getYoZ(), registry); + + return new SimpleMovingAverageFilteredYoFrameVector3D(x, y, z, unfilteredVector.getReferenceFrame()); + } + + @Override + public void update() + { + x.update(); + y.update(); + z.update(); + } + + public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered) + { + x.update(xUnfiltered); + y.update(yUnfiltered); + z.update(zUnfiltered); + } + + public void update(Vector3D vectorUnfiltered) + { + x.update(vectorUnfiltered.getX()); + y.update(vectorUnfiltered.getY()); + z.update(vectorUnfiltered.getZ()); + } + + public void update(FrameVector3D vectorUnfiltered) + { + checkReferenceFrameMatch(vectorUnfiltered); + x.update(vectorUnfiltered.getX()); + y.update(vectorUnfiltered.getY()); + z.update(vectorUnfiltered.getZ()); + } + + @Override + public void reset() + { + x.reset(); + y.reset(); + z.reset(); + } + + public boolean getHasBufferWindowFilled() + { + return x.getHasBufferWindowFilled() && y.getHasBufferWindowFilled() && z.getHasBufferWindowFilled(); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java new file mode 100644 index 00000000..243da69a --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java @@ -0,0 +1,138 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class AccelerationLimitedYoVariable extends YoDouble +{ + private final double dt; + + private final YoBoolean hasBeenInitialized; + + private final YoDouble smoothedRate; + private final YoDouble smoothedAcceleration; + + private final YoDouble positionGain; + private final YoDouble velocityGain; + + private DoubleProvider maximumRate; + private DoubleProvider maximumAcceleration; + + private final DoubleProvider inputVariable; + + public AccelerationLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, double dt) + { + this(name, registry, maxRate, maxAcceleration, null, dt); + } + + public AccelerationLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRate, DoubleProvider maxAcceleration, + DoubleProvider inputVariable, double dt) + { + super(name, registry); + + if (maxRate != null && maxAcceleration != null) + { + this.maximumRate = maxRate; + this.maximumAcceleration = maxAcceleration; + } + + this.dt = dt; + + hasBeenInitialized = new YoBoolean(name + "HasBeenInitialized", registry); + + smoothedRate = new YoDouble(name + "SmoothedRate", registry); + smoothedAcceleration = new YoDouble(name + "SmoothedAcceleration", registry); + + positionGain = new YoDouble(name + "PositionGain", registry); + velocityGain = new YoDouble(name + "VelocityGain", registry); + + double w0 = 2.0 * Math.PI * 16.0; + double zeta = 1.0; + + setGainsByPolePlacement(w0, zeta); + hasBeenInitialized.set(false); + + this.inputVariable = inputVariable; + } + + + public void setGainsByPolePlacement(double w0, double zeta) + { + positionGain.set(w0 * w0); + velocityGain.set(2.0 * zeta * w0); + } + + public YoDouble getPositionGain() + { + return positionGain; + } + + public YoDouble getVelocityGain() + { + return velocityGain; + } + + public void update() + { + update(inputVariable.getValue()); + } + + public void update(double input) + { + if (!hasBeenInitialized.getBooleanValue()) + initialize(input); + + double positionError = input - this.getDoubleValue(); + double acceleration = -velocityGain.getDoubleValue() * smoothedRate.getDoubleValue() + positionGain.getDoubleValue() * positionError; + acceleration = MathTools.clamp(acceleration, -maximumAcceleration.getValue(), maximumAcceleration.getValue()); + + smoothedAcceleration.set(acceleration); + smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); + smoothedRate.set(MathTools.clamp(smoothedRate.getDoubleValue(), maximumRate.getValue())); + this.add(smoothedRate.getDoubleValue() * dt); + } + + public void initialize(double input) + { + this.set(input); + smoothedRate.set(0.0); + smoothedAcceleration.set(0.0); + + this.hasBeenInitialized.set(true); + } + + public void reset() + { + this.hasBeenInitialized.set(false); + smoothedRate.set(0.0); + smoothedAcceleration.set(0.0); + } + + public YoDouble getSmoothedRate() + { + return smoothedRate; + } + + public YoDouble getSmoothedAcceleration() + { + return smoothedAcceleration; + } + + public boolean hasBeenInitialized() + { + return hasBeenInitialized.getBooleanValue(); + } + + public double getMaximumRate() + { + return maximumRate.getValue(); + } + + public double getMaximumAcceleration() + { + return maximumAcceleration.getValue(); + } +} \ No newline at end of file diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java new file mode 100644 index 00000000..836eeb91 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java @@ -0,0 +1,55 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.providers.DoubleProvider; + +/** + * This class calculates the alpha value given a break frequency provider + * + * The value gets cached to avoid unnecessary calculations + * + * @author Jesper Smith + * + */ +public class AlphaBasedOnBreakFrequencyProvider implements DoubleProvider +{ + + private final DoubleProvider breakFrequencyProvider; + private final double dt; + + private double previousBreakFrequency = Double.NaN; + private double alpha = 0.0; + + /** + * Create a new provider using the break frequency provided by double provider + * + * @param breakFrequencyProvider + * @param dt + */ + public AlphaBasedOnBreakFrequencyProvider(DoubleProvider breakFrequencyProvider, double dt) + { + this.breakFrequencyProvider = breakFrequencyProvider; + this.dt = dt; + } + + /** + * 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; + } + +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java new file mode 100644 index 00000000..12215c30 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java @@ -0,0 +1,33 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.commons.MathTools; + +public class AlphaFilterTools +{ + /** + * This computes the alpha + * variable for an alpha filtered yo variable that is equivalent to a first order low pass frequnecy at filter {@param breakFrequencyInHertz} + * + * @return alpha value + */ + public static double computeAlphaGivenBreakFrequencyProperly(double breakFrequencyInHertz, double dt) + { + if (Double.isInfinite(breakFrequencyInHertz)) + return 0.0; + + double omega = AngleTools.TwoPI * breakFrequencyInHertz; + double alpha = (1.0 - omega * dt / 2.0) / (1.0 + omega * dt / 2.0); + alpha = MathTools.clamp(alpha, 0.0, 1.0); + return alpha; + } + + /** + * This computes the break frequency of a first order low-pass filter given an alpha value. + * + * @return alpha value + */ + public static double computeBreakFrequencyGivenAlpha(double alpha, double dt) + { + return (1.0 - alpha) / (Math.PI * dt * (1.0 + alpha)); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java new file mode 100644 index 00000000..60a1bd07 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java @@ -0,0 +1,139 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class AlphaFilteredWrappingYoVariable extends AlphaFilteredYoVariable +{ + public static final double EPSILON = 1e-10; + + private double previousUnfilteredVariable; + private final YoDouble unfilteredVariable; + private final YoDouble unfilteredInRangeVariable; + private final DoubleProvider alphaVariable; + + private final YoDouble temporaryOutputVariable; + private final YoDouble error; + private final double upperLimit; + private final double lowerLimit; + private final double range; + + //wrap the values in [lowerLimit ; upperLimit[ + + public AlphaFilteredWrappingYoVariable(String name, String description, YoRegistry registry, final YoDouble unfilteredVariable, DoubleProvider alphaVariable, double lowerLimit, double upperLimit) + { + super(name, description, registry, alphaVariable); + this.alphaVariable = alphaVariable; + this.upperLimit = upperLimit; + this.lowerLimit = lowerLimit; + this.range = upperLimit - lowerLimit; + this.unfilteredVariable = unfilteredVariable; + + unfilteredInRangeVariable = new YoDouble(name + "UnfilteredInRangeVariable", registry); + temporaryOutputVariable = new YoDouble(name + "TemporaryOutputVariable", registry); + error = new YoDouble(name + "Error", registry); + + } + + @Override + public void update() + { + update(unfilteredVariable.getDoubleValue()); + } + + + @Override + public void update(double currentPosition) + { + if(!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + previousUnfilteredVariable = unfilteredVariable.getDoubleValue(); + + unfilteredVariableModulo(currentPosition); + + temporaryOutputVariable.set(unfilteredInRangeVariable.getDoubleValue()); + set(unfilteredInRangeVariable.getDoubleValue()); + } + else + { + if (!MathTools.epsilonEquals(currentPosition, previousUnfilteredVariable, EPSILON)) + { + previousUnfilteredVariable = currentPosition; + + unfilteredVariableModulo(currentPosition); + + //calculate the error + double standardError = unfilteredInRangeVariable.getDoubleValue() - getDoubleValue(); + double wrappingError; + if(unfilteredInRangeVariable.getDoubleValue() > getDoubleValue()) + { + wrappingError = lowerLimit - getDoubleValue() + unfilteredInRangeVariable.getDoubleValue() - upperLimit; + } + else + { + wrappingError = upperLimit - getDoubleValue() + unfilteredInRangeVariable.getDoubleValue() - lowerLimit; + } + if(Math.abs(standardError) < Math.abs(wrappingError)) + { + error.set(standardError); + } + else + { + error.set(wrappingError); + } + + //determine if wrapping and move the input if necessary + temporaryOutputVariable.set(getDoubleValue()); + if ((getDoubleValue() + error.getDoubleValue()) >= upperLimit) + { + temporaryOutputVariable.set(getDoubleValue() - range); + } + if ((getDoubleValue() + error.getDoubleValue()) < lowerLimit) + { + temporaryOutputVariable.set(getDoubleValue() + range); + } + } + + temporaryOutputVariable.set(alphaVariable.getValue() * temporaryOutputVariable.getDoubleValue() + (1.0 - alphaVariable.getValue()) + * unfilteredInRangeVariable.getDoubleValue()); + + if (temporaryOutputVariable.getDoubleValue() > upperLimit + EPSILON) + { + set(temporaryOutputVariable.getDoubleValue() - range); + } + else if (temporaryOutputVariable.getDoubleValue() <= lowerLimit - EPSILON) + { + set(temporaryOutputVariable.getDoubleValue() + range); + } + else + { + set(temporaryOutputVariable.getDoubleValue()); + } + } + } + + private void unfilteredVariableModulo(double currentPosition) + { + //handle if the input is out of range + boolean rangeNeedsToBeChecked = true; + unfilteredInRangeVariable.set(currentPosition); + + while(rangeNeedsToBeChecked) + { + rangeNeedsToBeChecked = false; + if (unfilteredInRangeVariable.getDoubleValue() >= upperLimit) + { + unfilteredInRangeVariable.set(unfilteredInRangeVariable.getDoubleValue() - range); + rangeNeedsToBeChecked = true; + } + if (unfilteredInRangeVariable.getDoubleValue() < lowerLimit) + { + unfilteredInRangeVariable.set(unfilteredInRangeVariable.getDoubleValue() + range); + rangeNeedsToBeChecked = true; + } + } + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java new file mode 100644 index 00000000..46e63224 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java @@ -0,0 +1,108 @@ +package us.ihmc.yoVariables.filters; + +import org.ejml.data.DMatrix; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class AlphaFilteredYoMatrix extends YoMatrix +{ + private final DMatrixRMaj previous; + private final DMatrixRMaj current; + private final DMatrixRMaj filtered; + + private final YoDouble alpha; + + public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, YoRegistry registry) + { + this(name, null, alpha, numberOfRows, numberOfColumns, null, null, registry); + } + + public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, YoRegistry registry) + { + this(name, null, alpha, numberOfRows, numberOfColumns, rowNames, null, registry); + } + + public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) + { + this(name, null, alpha, numberOfRows, numberOfColumns, rowNames, columnNames, registry); + } + + public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, YoRegistry registry) + { + this(name, description, alpha, numberOfRows, numberOfColumns, null, null, registry); + } + + public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, YoRegistry registry) + { + this(name, description, alpha, numberOfRows, numberOfColumns, rowNames, null, registry); + } + + public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) + { + super(name, description, numberOfRows, numberOfColumns, rowNames, columnNames, registry); + this.alpha = new YoDouble(name + "_alpha", registry); + this.alpha.set(alpha); + + previous = new DMatrixRMaj(numberOfRows, numberOfColumns); + current = new DMatrixRMaj(numberOfRows, numberOfColumns); + filtered = new DMatrixRMaj(numberOfRows, numberOfColumns); + } + + public void setAlpha(double alpha) + { + this.alpha.set(alpha); + } + + /** + * Set the current value of the matrix to be filtered. + *

+ * NOTE: This method does not solve for the filtered value. To solve for the filtered value, use {@link #solve()}. + *

+ * + * @param current the current value of the matrix to be filtered. Not modified. + */ + @Override + public void set(DMatrix current) + { + super.set(current); + this.current.set(current); + } + + /** + * Assuming that the current value has been set, this method solves for the filtered value. + *

+ * See {@link #set(DMatrix)} for how to set the matrix's current value. + *

+ */ + public void solve() + { + CommonOps_DDRM.scale(alpha.getDoubleValue(), previous, filtered); + + super.get(current); + CommonOps_DDRM.addEquals(filtered, 1 - alpha.getDoubleValue(), current); + + // Set the previous value to be the output of the filter, so it can be used next time + previous.set(filtered); + super.set(filtered); + } + + /** + * Set the current value of the matrix to be filtered and solve for the filtered value. + * + * @param current the current value of the matrix to be filtered. Not modified. + */ + public void setAndSolve(DMatrix current) + { + CommonOps_DDRM.scale(alpha.getDoubleValue(), previous, filtered); + + super.set(current); + this.current.set(current); + CommonOps_DDRM.addEquals(filtered, 1 - alpha.getDoubleValue(), this.current); + + // Set the previous value to be the output of the filter, so it can be used next time + previous.set(filtered); + super.set(filtered); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java new file mode 100644 index 00000000..0f7841fa --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java @@ -0,0 +1,172 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.euclid.tools.EuclidCoreTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +/** + * @author jrebula + *

+ *

+ * LittleDogVersion06: + * us.ihmc.LearningLocomotion.Version06.util.YoAlphaFilteredVariable, + * 9:34:00 AM, Aug 29, 2006 + *

= + *

+ * A YoAlphaFilteredVariable is a filtered version of an input YoVar. + * Either a YoVariable holding the unfiltered val is passed in to the + * constructor and update() is called every tick, or update(double) is + * called every tick. The YoAlphaFilteredVariable updates it's val + * with the current filtered version using + *

+ * 
+ *            filtered_{n} = alpha * filtered_{n-1} + (1 - alpha) * raw_{n}
+ *
+ * + * For alpha=0 -> no filtered + * For alpha=1 -> 100% filtered, no use of raw signal + */ +public class AlphaFilteredYoVariable extends YoDouble implements ProcessingYoVariable +{ + private final DoubleProvider alphaVariable; + + private final YoDouble position; + protected final YoBoolean hasBeenCalled; + + /** + * Please use + * @param namePrefix + * @param initialValue + * @param registry + * @return + */ + @Deprecated + public static DoubleProvider createAlphaYoDouble(String namePrefix, double initialValue, YoRegistry registry) + { + return VariableTools.createAlphaYoDouble(namePrefix, "", initialValue, registry); + } + + + public AlphaFilteredYoVariable(String name, YoRegistry registry, double alpha) + { + this(name, registry, alpha, null); + } + + public AlphaFilteredYoVariable(String name, YoRegistry registry, double alpha, YoDouble positionVariable) + { + this(name, "", registry, VariableTools.createAlphaYoDouble(name, "", alpha, registry), positionVariable); + } + + public AlphaFilteredYoVariable(String name, YoRegistry registry, DoubleProvider alphaVariable) + { + this(name, "", registry, alphaVariable, null); + } + + public AlphaFilteredYoVariable(String name, String description, YoRegistry registry, DoubleProvider alphaVariable) + { + this(name, description, registry, alphaVariable, null); + } + + public AlphaFilteredYoVariable(String name, YoRegistry registry, DoubleProvider alphaVariable, YoDouble positionVariable) + { + this(name, "", registry, alphaVariable, positionVariable); + } + + public AlphaFilteredYoVariable(String name, String description, YoRegistry registry, DoubleProvider alphaVariable, YoDouble positionVariable) + { + super(name, description, registry); + this.hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(name, "", registry); + this.position = positionVariable; + + if (alphaVariable == null) + alphaVariable = VariableTools.createAlphaYoDouble(name, "", 0.0, registry); + this.alphaVariable = alphaVariable; + + reset(); + } + + @Override + public void reset() + { + hasBeenCalled.set(false); + } + + @Override + public void update() + { + if (position == null) + { + throw new NullPointerException("YoAlphaFilteredVariable must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position.getDoubleValue()); + } + + public void update(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + set(currentPosition); + } + else + { + double alpha = alphaVariable.getValue(); + set(EuclidCoreTools.interpolate(currentPosition, getDoubleValue(), alpha)); + } + } + + /** + * This method is replaced by computeAlphaGivenBreakFrequencyProperly. It is fine to keep using this method is currently using it, knowing that + * the actual break frequency is not exactly what you are asking for. + * + * @param breakFrequencyInHertz + * @param dt + * @return + */ + @Deprecated + public static double computeAlphaGivenBreakFrequency(double breakFrequencyInHertz, double dt) + { + if (Double.isInfinite(breakFrequencyInHertz)) + return 0.0; + + double alpha = 1.0 - breakFrequencyInHertz * AngleTools.TwoPI * dt; + + alpha = MathTools.clamp(alpha, 0.0, 1.0); + + return alpha; + } + + /** + * This method has been replaced with {@link AlphaFilterTools#computeAlphaGivenBreakFrequencyProperly(double, double)}. It computes the alpha + * variable for an alpha filtered yo variable that is equivalent to a first order low pass frequnecy at filter {@param breakFrequencyInHertz} + * @return alpha value + */ + @Deprecated + public static double computeAlphaGivenBreakFrequencyProperly(double breakFrequencyInHertz, double dt) + { + return AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(breakFrequencyInHertz, dt); + } + + /** + * This method has been replaced with {@link AlphaFilterTools#computeBreakFrequencyGivenAlpha(double, double)}. + * This computes the break frequency of a first order low-pass filter given an alpha value. + * + * @return alpha value + */ + @Deprecated + public static double computeBreakFrequencyGivenAlpha(double alpha, double dt) + { + return AlphaFilterTools.computeBreakFrequencyGivenAlpha(alpha, dt); + } + + public boolean getHasBeenCalled() + { + return hasBeenCalled.getBooleanValue(); + } + +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AngleTools.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AngleTools.java new file mode 100644 index 00000000..e493d1d5 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AngleTools.java @@ -0,0 +1,399 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.euclid.tuple4D.Quaternion32; + +import java.util.Random; + +// TODO replace with teh values that are pushed into ihmc commons +@Deprecated +public class AngleTools +{ + + public static final double PI = Math.PI; + public static final double TwoPI = 2.0 * PI; + public static final double EPSILON = 1e-10; + + private AngleTools() + { + + } + + public static double trimAngleMinusPiToPi(double angle) + { + return AngleTools.shiftAngleToStartOfRange(angle, -PI); + } + + public static float getAngle(Quaternion32 q) + { + return 2.0f * (float) Math.acos(q.getS()); + } + + public static double angleMinusPiToPi(Vector2DReadOnly startVector, Vector2DReadOnly endVector) + { + double absoluteAngle = Math.acos(startVector.dot(endVector) / startVector.norm() / endVector.norm()); + + Vector3D start3d = new Vector3D(startVector.getX(), startVector.getY(), 0.0); + Vector3D end3d = new Vector3D(endVector.getX(), endVector.getY(), 0.0); + + Vector3D crossProduct = new Vector3D(); + crossProduct.cross(start3d, end3d); + + if (crossProduct.getZ() >= 0.0) + { + return absoluteAngle; + } + else + { + return -absoluteAngle; + } + } + + public static double computeAngleAverage(double angleA, double angleB) + { + // average = A + (B-A)/2 = (A+B)/2 + return interpolateAngle(angleA, angleB, 0.5); + } + + /** + * Performs a linear interpolation from {@code angleA} to {@code angleB} given the percentage + * {@code alpha} and trim the result to be in [-pi, pi]. + *
    + *
  • If {@code alpha == 0}, the result is {@code angleA}. + *
  • If {@code alpha == 1}, the result is {@code angleB}. + *
  • If {@code alpha == 0.5}, the result is the average of the two angles. + *
  • The percentage {@code alpha} is not clamped to be in [0, 1] such that this method can be used + * for extrapolation. + *
+ * + * @param angleA the first angle in the interpolation. + * @param angleB the second angle in the interpolation. + * @param alpha the percentage to use for the interpolation. A value of 0 will return + * {@code angleA}, while a value of 1 will return {@code angleB}. + * @return the interpolated angle in [-pi, pi]. + */ + public static double interpolateAngle(double angleA, double angleB, double alpha) + { + // A + alpha * (B-A)/2 + double average = angleA + alpha * computeAngleDifferenceMinusPiToPi(angleB, angleA); + return trimAngleMinusPiToPi(average); + } + + /** + * Formula found on Wikipedia. + */ + public static double computeAngleAverage(double[] angles) + { + double average = 0.0; + + double averageOfSin = 0.0; + double averageOfCos = 0.0; + double weight = 1.0 / angles.length; + + for (int i = 0; i < angles.length; i++) + { + averageOfSin += weight * Math.sin(angles[i]); + averageOfCos += weight * Math.cos(angles[i]); + } + + average = Math.atan2(averageOfSin, averageOfCos); + + return average; + } + + /** + * Finds the closest 90 degree yaw and returns number of 90 degrees (0 = 0; 1 = 90; 2 = 180; 3 = 270). + * + * @param yawInRadians double + * @return int between 0 and 3 for the number of 90 degree yawed. + */ + public static int findClosestNinetyDegreeYaw(double yawInRadians) + { + double minDifference = Double.POSITIVE_INFINITY; + int ret = -1; + + double difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, 0.0); + + minDifference = Math.abs(difference); + ret = 0; + + difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, Math.PI / 2.0); + + if (Math.abs(difference) < minDifference) + { + minDifference = Math.abs(difference); + ret = 1; + } + + difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, Math.PI); + + if (Math.abs(difference) < minDifference) + { + minDifference = Math.abs(difference); + ret = 2; + } + + difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, 3.0 * Math.PI / 2.0); + + if (Math.abs(difference) < minDifference) + { + minDifference = Math.abs(difference); + ret = 3; + } + + return ret; + } + + /** + * computeAngleDifferenceMinusPiToPi: returns (angleA - angleB), where the return value is [-pi, pi) + * + * @param angleA double + * @param angleB double + * @return double + */ + public static double computeAngleDifferenceMinusPiToPi(double angleA, double angleB) + { + double difference = angleA - angleB; + difference = difference % TwoPI; + difference = AngleTools.shiftAngleToStartOfRange(difference, -PI); + + return difference; + } + + /** + * computeAngleDifferenceMinusPiToPi: returns (angleA - angleB), where the return value is [-2.0*pi, 0.0) + * + * @param angleA double + * @param angleB double + * @return double + */ + public static double computeAngleDifferenceMinusTwoPiToZero(double angleA, double angleB) + { + double difference = angleA - angleB; + difference = difference % TwoPI; + difference = AngleTools.shiftAngleToStartOfRange(difference, -TwoPI); + + return difference; + } + + /** + * This will shift an angle to be in the range [startOfAngleRange, + * (startOfAngleRange + 2*pi) + * + * @param angleToShift the angle to shift + * @param startOfAngleRange start of the range. + * @return the shifted angle + */ + public static double shiftAngleToStartOfRange(double angleToShift, double startOfAngleRange) + { + return shiftAngleToStartOfRange(angleToShift, startOfAngleRange, TwoPI); + } + + /** + * This will shift an angle to be in the range [startOfAngleRange, + * (startOfAngleRange + endOfAngleRange) + * + * @param angleToShift the angle to shift + * @param startOfAngleRange start of the range. + * @param endOfAngleRange end of the range. + * @return the shifted angle + */ + public static double shiftAngleToStartOfRange(double angleToShift, double startOfAngleRange, double endOfAngleRange) + { + double ret = angleToShift; + startOfAngleRange = startOfAngleRange - EPSILON; + + if (angleToShift < startOfAngleRange) + { + ret = angleToShift + Math.ceil((startOfAngleRange - angleToShift) / endOfAngleRange) * endOfAngleRange; + } + + if (angleToShift >= (startOfAngleRange + endOfAngleRange)) + { + ret = angleToShift - Math.floor((angleToShift - startOfAngleRange) / endOfAngleRange) * endOfAngleRange; + } + + return ret; + } + + /** + * Returns an evenly distributed random number between -2PI and 2PI + * + * @param random Random number generator + * @return number between -2PI and 2PI + */ + public static double generateRandomAngle(Random random) + { + return -2.0 * Math.PI + 4 * Math.PI * random.nextDouble(); + } + + /** + * Returns array of angles increasing from -2PI to 2PI + * + * @param numberOfAngles + * @param stayThisFarAwayFromPlusMinus2PI + * @param includeZero + * @param includePlusMinusPI + * @return + */ + public static double[] generateArrayOfTestAngles(int numberOfAngles, double stayThisFarAwayFromPlusMinus2PI, boolean includeZero, boolean includePlusMinusPI) + { + int arraySize = numberOfAngles; + double epsilon = stayThisFarAwayFromPlusMinus2PI; + + if (includeZero && !includePlusMinusPI) + { + arraySize += 1; + } + else if (includePlusMinusPI && !includeZero) + { + arraySize += 2; + } + else if (includeZero && includePlusMinusPI) + { + arraySize += 3; + } + + double thetaMin = -2.0 * Math.PI + stayThisFarAwayFromPlusMinus2PI; + double thetaMax = 2.0 * Math.PI - stayThisFarAwayFromPlusMinus2PI; + double deltaTheta = Math.abs(thetaMax - thetaMin) / (numberOfAngles - 1); + + double[] ret = new double[arraySize]; + + for (int i = 0; i < numberOfAngles; i++) + { + ret[i] = thetaMin + deltaTheta * i; + + boolean epsilonEqualToZero = MathTools.epsilonEquals(Math.abs(ret[i]), 0.0, epsilon); + boolean epsilonEqualToPlusMinusPI = MathTools.epsilonEquals(Math.abs(ret[i]), Math.PI, epsilon); + + if (epsilonEqualToZero && !includeZero || epsilonEqualToPlusMinusPI && !includePlusMinusPI) + ret[i] += Math.signum(ret[i]) * Math.max(1.0e-4, Math.abs(epsilon)); + } + + if (includeZero && !includePlusMinusPI) + { + ret[numberOfAngles] = 0.0; + } + else if (includePlusMinusPI && !includeZero) + { + ret[numberOfAngles] = -Math.PI; + ret[numberOfAngles + 1] = Math.PI; + } + else if (includeZero && includePlusMinusPI) + { + ret[numberOfAngles] = 0.0; + ret[numberOfAngles + 1] = -Math.PI; + ret[numberOfAngles + 2] = Math.PI; + } + + return ret; + } + + /** + * Returns an angle between two points + heading Offset from -PI to PI. + * If the x or y components are both under the noTranslationTolerance, + * then the initial orientation as given in startPose will be returned. + * + * @param startPose initial position and orientation + * @param endPoint end position + * @param headingOffset offset from path angle + * @param noTranslationTolerance tolerance for determining if path angle should be determined + * @return number between -PI and PI + */ + public static double calculateHeading(FramePose2DReadOnly startPose, FramePoint2DReadOnly endPoint, double headingOffset, double noTranslationTolerance) + { + double deltaX = endPoint.getX() - startPose.getX(); + double deltaY = endPoint.getY() - startPose.getY(); + double heading; + if (Math.abs(deltaX) < noTranslationTolerance && Math.abs(deltaY) < noTranslationTolerance) + { + heading = startPose.getYaw(); + } + else + { + double pathHeading = Math.atan2(deltaY, deltaX); + heading = AngleTools.trimAngleMinusPiToPi(pathHeading + headingOffset); + } + return heading; + } + + /** + * Returns an angle between two points + heading Offset from -PI to PI. + * + * @param startPose initial position and orientation + * @param endPoint end position + * @param headingOffset offset from path angle + * @return number between -PI and PI + */ + public static double calculateHeading(Tuple3DReadOnly startPose, Tuple3DReadOnly endPoint, double headingOffset) + { + double deltaX = endPoint.getX() - startPose.getX(); + double deltaY = endPoint.getY() - startPose.getY(); + + double pathHeading = Math.atan2(deltaY, deltaX); + return AngleTools.trimAngleMinusPiToPi(pathHeading + headingOffset); + } + + /** + * Returns an angle between two points + heading Offset from -PI to PI. + * + * @param startPose initial position + * @param endPoint end position + * @return number between -PI and PI + */ + public static double calculateHeading(Point2DReadOnly startPose, Point2DReadOnly endPoint) + { + double deltaX = endPoint.getX() - startPose.getX(); + double deltaY = endPoint.getY() - startPose.getY(); + double heading; + + double pathHeading = Math.atan2(deltaY, deltaX); + heading = AngleTools.trimAngleMinusPiToPi(pathHeading); + + return heading; + } + + /** + * Pass in a vector. Get its angle in polar coordinates. + * + * @param vx + * @param vy + * @return angle of vector from 0 to 2PI + */ + public static double angleFromZeroToTwoPi(double vx, double vy) + { + double angleFromNegtivePiToPi = Math.atan2(vy, vx); + + if (angleFromNegtivePiToPi < 0.0) + { + return 2.0 * Math.PI + angleFromNegtivePiToPi; + } + else + { + return angleFromNegtivePiToPi; + } + } + + public static double roundToGivenPrecisionForAngle(double angleValue, double precisionFactor) + { + double centeredAngleValue = trimAngleMinusPiToPi(angleValue + 0.5 * precisionFactor); + long longValue = (long) (centeredAngleValue / precisionFactor); + double roundedValue = ((double) longValue) * precisionFactor; + return trimAngleMinusPiToPi(roundedValue); + } + + public static void roundToGivenPrecisionForAngles(Tuple3DBasics tuple3d, double precision) + { + tuple3d.setX(roundToGivenPrecisionForAngle(tuple3d.getX(), precision)); + tuple3d.setY(roundToGivenPrecisionForAngle(tuple3d.getY(), precision)); + tuple3d.setZ(roundToGivenPrecisionForAngle(tuple3d.getZ(), precision)); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java new file mode 100644 index 00000000..768fcabe --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java @@ -0,0 +1,202 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.euclid.tools.EuclidCoreTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoEnum; + +/** + * This variable is designed to compute the velocity of a position signal that may contain backlash. It computes the velocity of the position using a + * {@link FilteredFiniteDifferenceYoVariable}. When that velocity changes sign, the input is computed to have changed directions, and it enters a "slop" period. + * During the slop period, the output of this variable returns the previous velocity value, which is slowly scaled towards the new measured value over the course of + * the slop duration. + */ +public class BacklashCompensatingVelocityYoVariable extends YoDouble implements ProcessingYoVariable +{ + private final double dt; + + private final FilteredFiniteDifferenceYoVariable finiteDifferenceVelocity; + + private final DoubleProvider alphaVariable; + private final DoubleProvider position; + + private final YoDouble lastPosition; + private final YoBoolean hasBeenCalled; + + private final YoEnum backlashState; + private final DoubleProvider slopTime; + + private final YoDouble timeSinceSloppy; + + public BacklashCompensatingVelocityYoVariable(String name, + String description, + DoubleProvider alphaVariable, + DoubleProvider positionVariable, + double dt, + DoubleProvider slopTime, + YoRegistry registry) + { + + super(name, description, registry); + finiteDifferenceVelocity = new FilteredFiniteDifferenceYoVariable(name + "finiteDifferenceVelocity", "", alphaVariable, dt, registry); + + this.hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(name, "", registry); + + backlashState = new YoEnum<>(name + "BacklashState", registry, BacklashState.class, true); + backlashState.set(null); + timeSinceSloppy = new YoDouble(name + "TimeSinceSloppy", registry); + + position = positionVariable; + + this.alphaVariable = alphaVariable; + this.slopTime = slopTime; + + this.dt = dt; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + + reset(); + } + + public BacklashCompensatingVelocityYoVariable(String name, + String description, + YoDouble alphaVariable, + double dt, + YoDouble slopTime, + YoRegistry registry) + { + this(name, description, alphaVariable, null, dt, slopTime, registry); + } + + @Override + public void reset() + { + hasBeenCalled.set(false); + backlashState.set(null); + } + + @Override + public void update() + { + if (position == null) + { + throw new NullPointerException( + "BacklashCompensatingVelocityYoVariable must be constructed with a non null position variable to call update(), otherwise use update(double)"); + } + + update(position.getValue()); + } + + public void update(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + lastPosition.set(currentPosition); + set(0.0); + } + + finiteDifferenceVelocity.update(currentPosition); + double velocityFromFiniteDifferences = finiteDifferenceVelocity.getDoubleValue(); + + // increment time. + timeSinceSloppy.add(dt); + + // No estimate of the backlash state is available, so initialize it based on the current estimated velocity. + if (backlashState.getEnumValue() == null) + { + if (velocityFromFiniteDifferences < 0.0) + backlashState.set(BacklashState.BACKWARD_OK); + else if (velocityFromFiniteDifferences > 0.0) + backlashState.set(BacklashState.FORWARD_OK); + } + else + { + switch (backlashState.getEnumValue()) + { + case BACKWARD_OK: + { + if (velocityFromFiniteDifferences > 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.FORWARD_SLOP); + } + + break; + } + + case FORWARD_OK: + { + if (velocityFromFiniteDifferences < 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.BACKWARD_SLOP); + } + + break; + } + + case BACKWARD_SLOP: + { + if (velocityFromFiniteDifferences > 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.FORWARD_SLOP); + } + else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) + { + backlashState.set(BacklashState.BACKWARD_OK); + timeSinceSloppy.set(0.0); + } + + break; + } + + case FORWARD_SLOP: + { + if (velocityFromFiniteDifferences < 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.BACKWARD_SLOP); + } + else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) + { + backlashState.set(BacklashState.FORWARD_OK); + timeSinceSloppy.set(0.0); + } + + break; + } + } + } + + double difference = currentPosition - lastPosition.getDoubleValue(); + + // During the slop period, we want to increase the difference from 0 to 1, so that there is no velocity at the beginning of slop, and the velocity is + // increased to the current finite differenced value at the end of the slop period. + if (backlashState.getEnumValue() != null && backlashState.getEnumValue().isInBacklash()) + { + double alpha = timeSinceSloppy.getDoubleValue() / slopTime.getValue(); + alpha = MathTools.clamp(alpha, 0.0, 1.0); + if (Double.isNaN(alpha)) + alpha = 1.0; + + difference = alpha * difference; + } + + // The difference is then finite differenced and alpha filtered + updateUsingDifference(difference); + lastPosition.set(currentPosition); + } + + private void updateUsingDifference(double difference) + { + double previousFilteredDerivative = getDoubleValue(); + double currentRawDerivative = difference / dt; + + this.set(EuclidCoreTools.interpolate(currentRawDerivative, previousFilteredDerivative, alphaVariable.getValue())); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java new file mode 100644 index 00000000..8f8f6161 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java @@ -0,0 +1,150 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoEnum; + +/** + * This class is designed to perform an estimate of a velocity signal that may contain backlash. It does essentially the same as + * {@link BacklashCompensatingVelocityYoVariable}, except it takes a velocity signal as input. + * + * It works by zeroing out the estimated velocity whenever the finite-differenced velocity changes sign. It then ramps this value back up to the value returned + * by finite estimating over the course of the slop duration. It assumes that the slop time is some fixed, constant period when the estimate is unreliable. + */ +public class BacklashProcessingYoVariable extends YoDouble implements ProcessingYoVariable +{ + private final YoDouble velocity; + + private final YoBoolean hasBeenCalled; + + private final YoEnum backlashState; + private final DoubleProvider slopTime; + + private final YoDouble timeSinceSloppy; + + private final double dt; + + public BacklashProcessingYoVariable(String name, String description, double dt, DoubleProvider slopTime, YoRegistry registry) + { + this(name, description, null, dt, slopTime, registry); + } + + public BacklashProcessingYoVariable(String name, String description, YoDouble velocityVariable, double dt, DoubleProvider slopTime, YoRegistry registry) + { + super(name, description, registry); + + this.hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(name, "", registry); + + backlashState = new YoEnum<>(name + "BacklashState", registry, BacklashState.class, true); + backlashState.set(null); + timeSinceSloppy = new YoDouble(name + "TimeSinceSloppy", registry); + + velocity = velocityVariable; + + this.slopTime = slopTime; + + this.dt = dt; + + reset(); + } + + @Override + public void reset() + { + hasBeenCalled.set(false); + backlashState.set(null); + } + + @Override + public void update() + { + if (velocity == null) + { + throw new NullPointerException( + "BacklashProcessingYoVariable must be constructed with a non null velocity variable to call update(), otherwise use update(double)"); + } + + update(velocity.getDoubleValue()); + } + + public void update(double currentVelocity) + { + if (backlashState.getEnumValue() == null) + { + backlashState.set(BacklashState.FORWARD_OK); + } + + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + set(currentVelocity); + } + + timeSinceSloppy.add(dt); + + switch (backlashState.getEnumValue()) + { + case BACKWARD_OK: + { + if (currentVelocity > 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.FORWARD_SLOP); + } + + break; + } + + case FORWARD_OK: + { + if (currentVelocity < 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.BACKWARD_SLOP); + } + + break; + } + + case BACKWARD_SLOP: + { + if (currentVelocity > 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.FORWARD_SLOP); + } + else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) + { + backlashState.set(BacklashState.BACKWARD_OK); + } + + break; + } + + case FORWARD_SLOP: + { + if (currentVelocity < 0.0) + { + timeSinceSloppy.set(0.0); + backlashState.set(BacklashState.BACKWARD_SLOP); + } + else if (timeSinceSloppy.getDoubleValue() > slopTime.getValue()) + { + backlashState.set(BacklashState.FORWARD_OK); + } + + break; + } + } + + double percent = timeSinceSloppy.getDoubleValue() / slopTime.getValue(); + percent = MathTools.clamp(percent, 0.0, 1.0); + if (Double.isNaN(percent) || slopTime.getValue() < dt) + percent = 1.0; + + this.set(percent * currentVelocity); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashState.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashState.java new file mode 100644 index 00000000..29744ce7 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashState.java @@ -0,0 +1,18 @@ +package us.ihmc.yoVariables.filters; + +enum BacklashState +{ + BACKWARD_OK(false), FORWARD_OK(false), BACKWARD_SLOP(true), FORWARD_SLOP(true); + + private final boolean isInBacklash; + + private BacklashState(boolean inBacklash) + { + this.isInBacklash = inBacklash; + } + + public boolean isInBacklash() + { + return isInBacklash; + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandTools.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandTools.java new file mode 100644 index 00000000..98be17ac --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandTools.java @@ -0,0 +1,140 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics; +import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; +import us.ihmc.euclid.tuple2D.interfaces.Vector2DBasics; +import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; +import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; + +// TODO replace with teh values that are pushed into ihmc commons +@Deprecated +public class DeadbandTools +{ + /** + * Applies a deadband of size {@param deadbandSize} to value {@param value}, centered about zero. + *

+ * For example, if a deadband of 1 is applied to a signal of 3, the return is 2. If a deadband of 3 is applied to a signal of 3, the return is 0. + *

+ * + * @param deadbandSize size of deadband to apply + * @param value value to apply the deadband to. + * @return value after applying the deadband. + */ + public static double applyDeadband(double deadbandSize, double value) + { + return applyDeadband(deadbandSize, 0.0, value); + } + + /** + * Applies a deadband of size {@param deadbandSize} to value {@param value}, centered about {@param deadbandCenter}. + *

+ * For example, if a deadband of 1 is applied to a signal of 3 about 2, the return is 2. if a deadband of 3 is applied to a signal of 3 about 1, the return + * is 1. + *

+ * + * @param deadbandSize size of deadband to apply + * @param deadbandCenter center about which the deadband is applied. + * @param value value to apply the deadband to. + * @return value after applying the deadband. + */ + public static double applyDeadband(double deadbandSize, double deadbandCenter, double value) + { + if (value > deadbandCenter) + return Math.max(deadbandCenter, value - deadbandSize); + else + return Math.min(deadbandCenter, value + deadbandSize); + } + + /** + * Applies a deadband to the length of a vector. + * + * @param vectorToPack vector to apply the deadband to. + * @param deadband deadband magnitude to apply. + */ + public static void applyDeadband(Vector2DBasics vectorToPack, double deadband) + { + double length = vectorToPack.norm(); + if (length < deadband) + { + vectorToPack.setToZero(); + } + else + { + double newLength = length - deadband; + vectorToPack.scale(newLength / length); + } + } + + /** + * Applies a deadband to the length of a vector. + * + * @param vectorToPack vector to apply the deadband to. + * @param deadband deadband magnitude to apply. + */ + public static boolean applyDeadband(Vector3DBasics vectorToPack, double deadband) + { + double length = vectorToPack.norm(); + if (length < deadband) + { + vectorToPack.setToZero(); + return false; + } + else + { + double newLength = length - deadband; + vectorToPack.scale(newLength / length); + return true; + } + } + + /** + * Applies a deadband to the magnitude of a point from another point, where the total distance is the value that is deadbanded. This can be thought of as + * applying a deadband of the size {@param deadband} to the vector from {@param centerPoint} to {@param pointToPack}, and then using this deadbanded vector + * to recompute the value of {@param pointToPack}. + * + * @param pointToPack point to apply the deadband to. + * @param centerPoint point about which to apply the deadband. + * @param deadband deadband magnitude to apply. + */ + public static boolean applyDeadband(Point2DBasics pointToPack, Point2DReadOnly centerPoint, double deadband) + { + double distance = pointToPack.distance(centerPoint); + if (distance < deadband) + { + pointToPack.set(centerPoint); + return false; + } + else + { + double newDistance = distance - deadband; + pointToPack.interpolate(centerPoint, 1.0 - newDistance / distance); + return true; + } + } + + /** + * Applies a deadband to the magnitude of a point from another point, where the total distance is the value that is deadbanded. This can be thought of as + * applying a deadband of the size {@param deadband} to the vector from {@param centerPoint} to {@param pointToPack}, and then using this deadbanded vector + * to recompute the value of {@param pointToPack}. + * + * @param pointToPack point to apply the deadband to. + * @param centerPoint point about which to apply the deadband. + * @param deadband deadband magnitude to apply. + */ + public static boolean applyDeadband(Point3DBasics pointToPack, Point3DReadOnly centerPoint, double deadband) + { + double distance = pointToPack.distance(centerPoint); + if (distance < deadband) + { + pointToPack.set(centerPoint); + return false; + } + else + { + double newDistance = distance - deadband; + pointToPack.interpolate(centerPoint, 1.0 - newDistance / distance); + return true; + } + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java new file mode 100644 index 00000000..12670240 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java @@ -0,0 +1,59 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class DeadbandedYoVariable extends YoDouble implements ProcessingYoVariable +{ + private final DoubleProvider deadzoneSize; + private final DoubleProvider inputVariable; + + public DeadbandedYoVariable(String name, DoubleProvider deadzoneSize, YoRegistry registry) + { + super(name, registry); + this.inputVariable = null; + this.deadzoneSize = deadzoneSize; + } + + public DeadbandedYoVariable(String name, DoubleProvider inputVariable, DoubleProvider deadzoneSize, YoRegistry registry) + { + super(name, registry); + this.inputVariable = inputVariable; + this.deadzoneSize = deadzoneSize; + } + + public void update() + { + if (inputVariable == null) + throw new NullPointerException("DeadzoneYoVariable must be constructed with a non null " + + "input variable to call update(), otherwise use update(double)"); + update(inputVariable.getValue()); + } + + public void update(double valueToBeCorrected) + { + super.set(DeadbandTools.applyDeadband(deadzoneSize.getValue(), valueToBeCorrected)); + } + + public static void main(String[] args) + { + YoRegistry registry = new YoRegistry("test"); + YoDouble deadzoneSize = new YoDouble("deadzoneSize", registry); + YoDouble input = new YoDouble("input", registry); + deadzoneSize.set(2.0); + DeadbandedYoVariable testDeadzone = new DeadbandedYoVariable("testDeadZone", input, deadzoneSize, registry); + + for(int i = -50; i < 51; i++) + { + input.set((double)i); + testDeadzone.update(); + System.out.println("//////////////////////////"); + System.out.println("uncorrected = " + (double)i); + System.out.println("corrected = " + testDeadzone.getDoubleValue()); + } + + System.out.println("done"); + } + +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java new file mode 100644 index 00000000..3d18c14f --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java @@ -0,0 +1,72 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +/** + * This is a class that introduces purposeful delay between the passed in variable and the output of this variable. This is useful to capture latency in a + * process. The length of the delay is set by a specific number of ticks. + */ +public class DelayedYoBoolean extends YoBoolean +{ + private final YoBoolean variableToDelay; + + private final YoBoolean[] previousYoVariables; + + public DelayedYoBoolean(String name, String description, YoBoolean variableToDelay, int ticksToDelay, YoRegistry registry) + { + super(name, description, registry); + + this.variableToDelay = variableToDelay; + previousYoVariables = new YoBoolean[ticksToDelay]; + + for (int i = 0; i < ticksToDelay; i++) + { + previousYoVariables[i] = new YoBoolean(name + "_previous" + i, registry); + previousYoVariables[i].set(variableToDelay.getBooleanValue()); + } + + this.set(variableToDelay.getBooleanValue()); + } + + public void update() + { + if (previousYoVariables.length == 0) + { + this.set(variableToDelay.getBooleanValue()); + return; + } + + this.set(previousYoVariables[0].getBooleanValue()); + + for (int i = 0; i < previousYoVariables.length - 1; i++) + { + previousYoVariables[i].set(previousYoVariables[i + 1].getBooleanValue()); + } + + previousYoVariables[previousYoVariables.length - 1].set(variableToDelay.getBooleanValue()); + } + + public void reset() + { + for (YoBoolean var : previousYoVariables) + { + var.set(variableToDelay.getBooleanValue()); + } + this.set(variableToDelay.getBooleanValue()); + } + + void getInternalState(String inputString, Boolean ifDebug) + { + if (!ifDebug) + return; + + String string = inputString + "\nvalue = " + this.getBooleanValue() + "\n"; + for (int i = 0; i < previousYoVariables.length; i++) + { + string = string + i + " = " + previousYoVariables[i].getBooleanValue() + "\n"; + } + string = string + "variableToDelay = " + variableToDelay.getBooleanValue() + "\n"; + System.out.println(string); + } +} \ No newline at end of file diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java new file mode 100644 index 00000000..47301666 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java @@ -0,0 +1,59 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +/** + * This is a class that introduces purposeful delay between the passed in variable and the output of this variable. This is useful to capture latency in a + * process. The length of the delay is set by a specific number of ticks. + */ +public class DelayedYoDouble extends YoDouble +{ + private final DoubleProvider variableToDelay; + + private final YoDouble[] previousYoDouble; + + public DelayedYoDouble(String name, String description, DoubleProvider variableToDelay, int ticksToDelay, YoRegistry registry) + { + super(name, description, registry); + + this.variableToDelay = variableToDelay; + previousYoDouble = new YoDouble[ticksToDelay]; + + for (int i = 0; i < ticksToDelay; i++) + { + previousYoDouble[i] = new YoDouble(name + "_previous" + i, registry); + previousYoDouble[i].set(variableToDelay.getValue()); + } + + this.set(variableToDelay.getValue()); + } + + public void update() + { + if (previousYoDouble.length == 0) + { + this.set(variableToDelay.getValue()); + return; + } + + this.set(previousYoDouble[0].getValue()); + + for (int i = 0; i < previousYoDouble.length - 1; i++) + { + previousYoDouble[i].set(previousYoDouble[i + 1].getValue()); + } + + previousYoDouble[previousYoDouble.length - 1].set(variableToDelay.getValue()); + } + + public void reset() + { + for (YoDouble var : previousYoDouble) + { + var.set(variableToDelay.getValue()); + } + this.set(variableToDelay.getValue()); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java new file mode 100644 index 00000000..688363d1 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java @@ -0,0 +1,68 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +/** + * This variable stores data in such a way that its value is guaranteed to be within a certain delta of the desired value. + */ +public class DeltaLimitedYoVariable extends YoDouble +{ + private final YoDouble maxDelta; + private final YoDouble actual; + private final YoDouble desired; + private final YoBoolean isLimitingActive; + + public DeltaLimitedYoVariable(String name, YoRegistry registry, double maxDelta) + { + super(name, registry); + + this.maxDelta = new YoDouble(name + "MaxAllowedDelta", registry); + this.maxDelta.set(Math.abs(maxDelta)); + this.actual = new YoDouble(name + "Actual", registry); + this.desired = new YoDouble(name + "Desired", registry); + this.isLimitingActive = new YoBoolean(name + "IsLimitingActive", registry); + isLimitingActive.set(false); + } + + public void setMaxDelta(double maxDelta) + { + this.maxDelta.set(Math.abs(maxDelta)); + } + + public void updateOutput(double actual, double desired) + { + this.desired.set(desired); + this.actual.set(actual); + updateOutput(); + } + + public boolean isLimitingActive() + { + return isLimitingActive.getBooleanValue(); + } + + private void updateOutput() + { + double actualDoubleValue = actual.getDoubleValue(); + double desiredDoubleValue = desired.getDoubleValue(); + double maxDeltaDoubleValue = Math.abs(maxDelta.getDoubleValue()); + double rawDelta = actualDoubleValue - desiredDoubleValue; + double sign = Math.signum(rawDelta); + double requestedDelta = Math.abs(rawDelta); + double overshoot = maxDeltaDoubleValue - requestedDelta; + + if(overshoot < 0) + { + desiredDoubleValue = actualDoubleValue - maxDeltaDoubleValue * sign; + isLimitingActive.set(true); + } + else + { + isLimitingActive.set(false); + } + + this.set(desiredDoubleValue); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java new file mode 100644 index 00000000..23c41014 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java @@ -0,0 +1,168 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.euclid.tools.EuclidCoreTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoEnum; + +/** + * @author jrebula + *

+ * A {@link FilteredDiscreteVelocityYoVariable} is a filtered velocity of a position. That is, it takes the finite difference of the position signal to + * compute a velocity, it then filters the velocity using an alpha filter + * Either a YoVariable holding the position is passed in to the + * constructor and update() is called every tick, or update(double) is + * called every tick. The YoFilteredVelocityVariable updates it's val + * with the current velocity after a filter of + *

+ * + * 
+ *            vel_{n} = alpha * vel{n-1} + (1 - alpha) * (pos_{n} - pos_{n-1})
+ *
+ * + */ +public class FilteredDiscreteVelocityYoVariable extends YoDouble +{ + + private final YoDouble time; + + private final DoubleProvider alphaVariable; + private final YoDouble position; + + private final YoDouble lastUpdateTime; + private final YoEnum lastUpdateDirection; + private final YoDouble unfilteredVelocity; + + private final YoDouble lastPosition; + private boolean hasBeenCalled; + + public FilteredDiscreteVelocityYoVariable(String name, String description, double alpha, YoDouble time, YoRegistry registry) + { + this(name, description, alpha, null, time, registry); + } + + public FilteredDiscreteVelocityYoVariable(String name, String description, double alpha, YoDouble positionVariable, YoDouble time, + YoRegistry registry) + { + this(name, description, VariableTools.createAlphaYoDouble(name, "", alpha, registry), positionVariable, time, registry); + } + + public FilteredDiscreteVelocityYoVariable(String name, String description, DoubleProvider alphaVariable, YoDouble positionVariable, + YoDouble time, YoRegistry registry) + { + super(name, description, registry); + position = positionVariable; + this.alphaVariable = alphaVariable; + + this.time = time; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + lastUpdateTime = new YoDouble(name + "_lastUpdateTime", registry); + lastUpdateDirection = new YoEnum<>(name + "_lastUpdateDirection", registry, Direction.class); + unfilteredVelocity = new YoDouble(name + "_unfilteredVelocity", registry); + + reset(); + } + + public void reset() + { + hasBeenCalled = false; + } + + public void update() + { + if (position == null) + { + throw new NullPointerException("YoFilteredVelocityVariable must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position.getDoubleValue()); + } + + public void update(double currentPosition) + { + if (!hasBeenCalled) + { + hasBeenCalled = true; + + // lastPosition = currentPosition; + lastPosition.set(currentPosition); + lastUpdateTime.set(time.getDoubleValue()); + lastUpdateDirection.set(Direction.NONE); + } + + // Figure out if the count changed and if so, if the direction changed or not and then update the direction: + boolean countChanged = false; + if (currentPosition != lastPosition.getDoubleValue()) + countChanged = true; + + // If the count changed, figure out if the direction changed: + + boolean directionChanged = false; + if (countChanged) + { + if (currentPosition > lastPosition.getDoubleValue()) + { + if (lastUpdateDirection.getEnumValue() != Direction.FORWARD) + directionChanged = true; + lastUpdateDirection.set(Direction.FORWARD); + } + else if (currentPosition < lastPosition.getDoubleValue()) + { + if (lastUpdateDirection.getEnumValue() != Direction.BACKWARD) + directionChanged = true; + lastUpdateDirection.set(Direction.BACKWARD); + } + } + + // If the direction changed, then the velocity is set to zero: + if (directionChanged) + { + unfilteredVelocity.set(0.0); + } + + // If the direction hasn't changed, but the count changed then compute the velocity based on the time since last update: + else if (countChanged) + { + double diffTime = time.getDoubleValue() - lastUpdateTime.getDoubleValue(); + if (diffTime < 1e-7) + unfilteredVelocity.set(0.0); + else + { + unfilteredVelocity.set((currentPosition - lastPosition.getDoubleValue()) / diffTime); + } + } + + else + { + // If the count hasn't changed, then not quite sure what the velocity is. + // We could just use the current velocity, but really should try to figure out if things have been slowing down or not. + // For now, multiply by some largish fraction, just to make sure it does trail off to zero if the velocity stops quickly. + unfilteredVelocity.set(0.99 * unfilteredVelocity.getDoubleValue()); + } + + // Low pass alpha filter it... + set(EuclidCoreTools.interpolate(unfilteredVelocity.getDoubleValue(), getDoubleValue(), alphaVariable.getValue())); + + // Remember the position and the currentTime if the countChanged: + + if (countChanged) + { + lastPosition.set(currentPosition); + lastUpdateTime.set(time.getDoubleValue()); + } + } + + public double getUnfilteredVelocity() + { + return unfilteredVelocity.getDoubleValue(); + } + + private enum Direction + { + NONE, FORWARD, BACKWARD; + } + +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java new file mode 100644 index 00000000..5c0db233 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java @@ -0,0 +1,140 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +/** + *

+ * A {@link FilteredFiniteDifferenceYoVariable} computes the velocity of a position signal via finite differencing it, and then applies an alpha filter to that + * value. The value contained in the parent {@link YoDouble} returns the filtered velocity signal. + *

+ *

+ * To construct and use this variable, either a YoVariable holding the position is passed in at construction + * constructor and {@link #update()} is called every tick, or {@link #update(double)} is + * called every tick. + *

+ *

The {@link FilteredFiniteDifferenceYoVariable} updates its value with the current velocity after a filter of + *

+ * + * 
+ *                  vel_{n} = alpha * vel{n-1} + (1 - alpha) * (pos_{n} - pos_{n-1})
+ *
+ */ +public class FilteredFiniteDifferenceYoVariable extends YoDouble implements ProcessingYoVariable +{ + private final double dt; + + private final DoubleProvider alphaVariable; + private final DoubleProvider position; + + // private double lastPosition; + private final YoDouble lastPosition; + private final YoBoolean hasBeenCalled; + + public FilteredFiniteDifferenceYoVariable(String name, String description, double alpha, double dt, YoRegistry registry) + { + this(name, description, alpha, null, dt, registry); + } + + public FilteredFiniteDifferenceYoVariable(String name, String description, double alpha, DoubleProvider positionVariable, double dt, YoRegistry registry) + { + this(name, description, VariableTools.createAlphaYoDouble(name, "", alpha, registry), positionVariable, dt, registry); + } + + public FilteredFiniteDifferenceYoVariable(String name, String description, DoubleProvider alphaVariable, double dt, YoRegistry registry) + { + this(name, description, alphaVariable, null, dt, registry); + } + + public FilteredFiniteDifferenceYoVariable(String name, + String description, + DoubleProvider alphaVariable, + DoubleProvider positionVariable, + double dt, + YoRegistry registry) + { + super(name, description, registry); + this.hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(name, "", registry); + + position = positionVariable; + this.alphaVariable = alphaVariable; + + this.dt = dt; + + lastPosition = new YoDouble(name + "_lastPosition", registry); + + reset(); + } + + @Override + public void reset() + { + hasBeenCalled.set(false); + } + + @Override + public void update() + { + if (position == null) + { + throw new NullPointerException( + "YoFilteredVelocityVariable must be constructed with a non null " + "position variable to call update(), otherwise use update(double)"); + } + + update(position.getValue()); + } + + public void updateForAngles() + { + if (position == null) + { + throw new NullPointerException( + "YoFilteredVelocityVariable must be constructed with a non null " + "position variable to call update(), otherwise use update(double)"); + } + + updateForAngles(position.getValue()); + } + + public void update(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + lastPosition.set(currentPosition); + set(0.0); + } + + double difference = currentPosition - lastPosition.getDoubleValue(); + + updateUsingDifference(difference); + + lastPosition.set(currentPosition); + } + + public void updateForAngles(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + lastPosition.set(currentPosition); + set(0.0); + } + + double difference = AngleTools.computeAngleDifferenceMinusPiToPi(currentPosition, lastPosition.getDoubleValue()); + + updateUsingDifference(difference); + + lastPosition.set(currentPosition); + } + + private void updateUsingDifference(double difference) + { + double previousFilteredDerivative = getDoubleValue(); + double currentRawDerivative = difference / dt; + + double alpha = alphaVariable.getValue(); + set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java new file mode 100644 index 00000000..08596c2b --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java @@ -0,0 +1,185 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class FirstOrderBandPassFilteredYoDouble extends YoDouble +{ + private final YoBoolean hasBeenCalled; + + private double filterUpdateTimeOld; + private FirstOrderFilterType filterType; + + private final YoDouble firstCutoffFrequencyHz; + private final YoDouble secondCutoffFrequencyHz; + + private final DoubleProvider yoTime; + private double dt; + + + public enum FirstOrderFilterType + { + NOTCH, BAND + } + + public FirstOrderBandPassFilteredYoDouble(String name, + String description, + double minPassThroughFrequency_Hz, + double maxPassThroughFrequency_Hz, + DoubleProvider yoTime, + FirstOrderFilterType filterType, + YoRegistry registry) + { + this(name, description, minPassThroughFrequency_Hz, maxPassThroughFrequency_Hz, yoTime, 0.0, filterType, registry); + } + + public FirstOrderBandPassFilteredYoDouble(String name, + String description, + double minPassThroughFrequency_Hz, + double maxPassThroughFrequency_Hz, + double dt, + FirstOrderFilterType filterType, + YoRegistry registry) + { + this(name, description, minPassThroughFrequency_Hz, maxPassThroughFrequency_Hz, null, dt, filterType, registry); + } + + private FirstOrderBandPassFilteredYoDouble(String name, + String description, + double minPassThroughFrequency_Hz, + double maxPassThroughFrequency_Hz, + DoubleProvider yoTime, + double dt, + FirstOrderFilterType filterType, + YoRegistry registry) + { + super(name, description, registry); + + String firstCutoffFrequencyName, secondCuttoffFrequencyName; + switch (filterType) + { + case NOTCH: + firstCutoffFrequencyName = name + "_NotchPassStart_Hz"; + secondCuttoffFrequencyName = name + "_NotchPassEnd_Hz"; + break; + case BAND: + firstCutoffFrequencyName = name + "_BandPassStart_Hz"; + secondCuttoffFrequencyName = name + "_BandPassEnd_Hz"; + break; + default: + throw new RuntimeException("Must specify filter type notch or break"); + } + + hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(name, "", registry); + hasBeenCalled.set(false); + this.firstCutoffFrequencyHz = new YoDouble(firstCutoffFrequencyName, registry); + this.firstCutoffFrequencyHz.set(minPassThroughFrequency_Hz); + this.secondCutoffFrequencyHz = new YoDouble(secondCuttoffFrequencyName, registry); + this.secondCutoffFrequencyHz.set(maxPassThroughFrequency_Hz); + + this.yoTime = yoTime; + this.dt = dt; + + this.filterType = filterType; + } + + private void checkPassband(double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz) + { + if (minPassThroughFrequency_Hz > maxPassThroughFrequency_Hz) + { + throw new RuntimeException( + "minPassThroughFrequency [ " + minPassThroughFrequency_Hz + " ] > maxPassThroughFrequency [ " + maxPassThroughFrequency_Hz + " ]"); + } + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update(double filterInput) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + this.set(filterInput); + } + else + { + if (yoTime != null) + { + double timeSinceLastUpdate = yoTime.getValue() - filterUpdateTimeOld; + + if (timeSinceLastUpdate > 0.0) + { + dt = timeSinceLastUpdate; + } + else + { + reset(); + // throw new RuntimeException("Computed step size, DT must be greater than zero. DT = " + dt + ". Current time = " + yoTime.getDoubleValue() + ", previous update time = " + filterUpdateTimeOld); + } + } + double filterOutput; + + switch (filterType) + { + case NOTCH: + filterOutput = applyNotchFilter(filterInput, dt); + break; + case BAND: + filterOutput = applyBandFilter(filterInput, dt); + break; + default: + throw new RuntimeException("The first order filter must be either a high pass or low pass filter, it cannot be " + filterType); + } + + this.set(filterOutput); + } + + if (yoTime != null) + { + filterUpdateTimeOld = yoTime.getValue(); + } + } + + public void setPassBand(double minPassThroughFreqHz, double maxPassThroughFreqHz) + { + checkPassband(minPassThroughFreqHz, maxPassThroughFreqHz); + + firstCutoffFrequencyHz.set(minPassThroughFreqHz); + secondCutoffFrequencyHz.set(maxPassThroughFreqHz); + } + + private double applyNotchFilter(double filterInput, double dt) + { + double lowFiltered = applyLowPassFilter(filterInput, firstCutoffFrequencyHz.getDoubleValue(), dt); + double highFiltered = applyHighPassFilter(filterInput, secondCutoffFrequencyHz.getDoubleValue(), dt); + + return lowFiltered + highFiltered; + } + + private double applyBandFilter(double filterInput, double dt) + { + double lowFiltered = applyLowPassFilter(filterInput, firstCutoffFrequencyHz.getDoubleValue(), dt); + double highFiltered = applyHighPassFilter(filterInput, secondCutoffFrequencyHz.getDoubleValue(), dt); + + return filterInput - lowFiltered - highFiltered; + } + + private double applyLowPassFilter(double filterInput, double breakFrequency, double dt) + { + double alpha = AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(dt, breakFrequency); + + return alpha * this.getDoubleValue() + (1.0 - alpha) * filterInput; + } + + private double applyHighPassFilter(double filterInput, double breakFrequency, double dt) + { + double lowPassValue = applyLowPassFilter(filterInput, breakFrequency, dt); + + return filterInput - lowPassValue; + } +} \ No newline at end of file diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java new file mode 100644 index 00000000..ef26ebab --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java @@ -0,0 +1,140 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class FirstOrderFilteredYoDouble extends YoDouble +{ + public enum FirstOrderFilterType + { + LOW_PASS, NOTCH, BAND, HIGH_PASS + } + + private boolean hasBeenCalled = false; + + private double filterUpdateTimeOld; + + private final YoDouble cutoffFrequencyHz; + + private final DoubleProvider yoTime; + private double dt; + + private FirstOrderFilterType filterType; + + public FirstOrderFilteredYoDouble(String name, + String description, + double cutoffFrequencyHz, + DoubleProvider yoTime, + FirstOrderFilterType highOrLowPass, + YoRegistry registry) + { + super(name, description, registry); + + String cutoffFrequencyName; + switch (highOrLowPass) + { + case LOW_PASS: + cutoffFrequencyName = name + "_LowPassCutoff_Hz"; + break; + case HIGH_PASS: + cutoffFrequencyName = name + "_HighPassCutoff_Hz"; + break; + default: + throw new RuntimeException("Must Specify Filter Type as Low or High Pass. Current Specification : " + highOrLowPass); + } + + this.cutoffFrequencyHz = new YoDouble(cutoffFrequencyName, registry); + this.cutoffFrequencyHz.set(cutoffFrequencyHz); + + this.yoTime = yoTime; + + this.filterType = highOrLowPass; + } + + public FirstOrderFilteredYoDouble(String name, + String description, + double cutoffFrequency_Hz, + double dt, + FirstOrderFilterType highOrLowPass, + YoRegistry registry) + { + this(name, description, cutoffFrequency_Hz, null, highOrLowPass, registry); + this.dt = dt; + } + + private double computeLowPassUpdate(double filterInput, double dt) + { + double alpha = AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(dt, cutoffFrequencyHz.getDoubleValue()); + + return alpha * this.getDoubleValue() + (1.0 - alpha) * filterInput; + } + + private double computeHighPassUpdate(double filterInput, double dt) + { + double lowPassValue = computeLowPassUpdate(filterInput, dt); + + return filterInput - lowPassValue; + } + + public void reset() + { + hasBeenCalled = false; + } + + public void setCutoffFrequencyHz(double cutoffHz) + { + this.cutoffFrequencyHz.set(cutoffHz); + } + + public void update(double filterInput) + { + if (!hasBeenCalled) + { + hasBeenCalled = true; + + filterUpdateTimeOld = 0.0; + + this.set(filterInput); + } + else + { + if (yoTime != null) + { + double timeSinceLastUpdate = yoTime.getValue() - filterUpdateTimeOld; + + if (timeSinceLastUpdate > 0.0) + { + dt = timeSinceLastUpdate; + } + else + { + reset(); + // throw new RuntimeException("Computed step size, DT must be greater than zero. DT = " + dt + ". Current time = " + yoTime.getDoubleValue() + ", previous update time = " + filterUpdateTimeOld); + } + } + + double filterOutput; + + switch (filterType) + { + case LOW_PASS: + filterOutput = computeLowPassUpdate(filterInput, dt); + break; + case HIGH_PASS: + filterOutput = computeHighPassUpdate(filterInput, dt); + break; + default: + throw new RuntimeException("The first order filter must be either a high pass or low pass filter, it cannot be " + filterType); + } + + this.set(filterOutput); + } + + + if (yoTime != null) + { + filterUpdateTimeOld = yoTime.getValue(); + } + } +} \ No newline at end of file diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java new file mode 100644 index 00000000..c2f6d2f0 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java @@ -0,0 +1,92 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoInteger; + +public class GlitchFilteredYoBoolean extends YoBoolean +{ + private final YoBoolean variableToFilter; + private final YoInteger windowSize; + protected final YoInteger counter; + + public GlitchFilteredYoBoolean(String name, YoRegistry registry, int windowSize) + { + this(name, "", registry, null, windowSize); + } + + public GlitchFilteredYoBoolean(String name, YoRegistry registry, YoBoolean yoVariableToFilter, int windowSize) + { + this(name, "", registry, yoVariableToFilter, windowSize); + } + + public GlitchFilteredYoBoolean(String name, YoRegistry registry, YoBoolean yoVariableToFilter, YoInteger windowSize) + { + this(name, "", registry, yoVariableToFilter, windowSize); + } + + public GlitchFilteredYoBoolean(String name, String description, YoRegistry registry, YoBoolean yoVariableToFilter, int windowSize) + { + this(name, description, registry, yoVariableToFilter, VariableTools.createWindowSizeYoInteger(name, "", windowSize, registry)); + } + + public GlitchFilteredYoBoolean(String name, String description, YoRegistry registry, YoBoolean yoVariableToFilter, YoInteger windowSize) + { + super(name, description, registry); + counter = new YoInteger(name + "Count", description, registry); + this.windowSize = windowSize; + + if (windowSize.getIntegerValue() < 0) + throw new RuntimeException("window size must be greater than 0"); + + variableToFilter = yoVariableToFilter; + + if (variableToFilter != null) + this.set(yoVariableToFilter.getBooleanValue()); + + this.set(false); + } + + public boolean set(boolean value) + { + if (counter != null) + { + counter.set(0); + } + return super.set(value); + } + + public void update(boolean value) + { + + if (value != this.getBooleanValue()) + { + counter.set(counter.getIntegerValue() + 1); + } + else + counter.set(0); + + if (counter.getIntegerValue() >= (windowSize.getIntegerValue())) + set(value); + } + + public int getWindowSize() + { + return windowSize.getIntegerValue(); + } + + public void setWindowSize(int windowSize) //untested + { + this.windowSize.set(windowSize); + } + + public void update() + { + if (variableToFilter == null) + { + throw new RuntimeException("variableToFilter was not initialized. Use the other constructor"); + } + else + update(variableToFilter.getBooleanValue()); + } +} \ No newline at end of file diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java new file mode 100644 index 00000000..469e7a70 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java @@ -0,0 +1,99 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.providers.IntegerProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoInteger; + +public class GlitchFilteredYoInteger extends YoInteger +{ + private final IntegerProvider position; + private final YoInteger previousPosition; + private final IntegerProvider windowSize; + private final YoInteger counter; + + public GlitchFilteredYoInteger(String name, int windowSize, YoRegistry registry) + { + this(name, windowSize, null, registry); + } + + public GlitchFilteredYoInteger(String name, int windowSize, IntegerProvider position, YoRegistry registry) + { + this(name, VariableTools.createWindowSizeYoInteger(name, "", windowSize, registry), position, registry); + } + + public GlitchFilteredYoInteger(String name, IntegerProvider windowSize, YoRegistry registry) + { + this(name, windowSize, null, registry); + } + + public GlitchFilteredYoInteger(String name, IntegerProvider windowSize, IntegerProvider position, YoRegistry registry) + { + super(name, GlitchFilteredYoInteger.class.getSimpleName(), registry); + + this.position = position; + + previousPosition = new YoInteger(name + "PrevValue", registry); + counter = new YoInteger(name + "Count", registry); + this.windowSize = windowSize; + } + + @Override + public boolean set(int value) + { + if (counter != null) + counter.set(0); + return super.set(value); + } + + @Override + public boolean set(int value, boolean notifyListeners) + { + if (counter != null) + counter.set(0); + return super.set(value, notifyListeners); + } + + public void update() + { + if (position == null) + { + throw new NullPointerException( + "GlitchFilteredYoInteger must be constructed with a non null position variable to call update(), otherwise use update(int)"); + } + + update(position.getValue()); + } + + public void update(int currentValue) + { + if (currentValue == previousPosition.getIntegerValue()) + counter.increment(); + else + counter.set(0); + + if (counter.getIntegerValue() >= windowSize.getValue()) + { + set(currentValue); + counter.set(0); + } + + previousPosition.set(currentValue); + } + + public int getWindowSize() + { + return windowSize.getValue(); + } + + public void setWindowSize(int windowSize) + { + if (this.windowSize instanceof YoInteger) + { + ((YoInteger) this.windowSize).set(windowSize); + } + else + { + throw new RuntimeException("Setting the window size is not supported"); + } + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java new file mode 100644 index 00000000..8f66d063 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java @@ -0,0 +1,151 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class JerkLimitedYoDouble extends YoDouble +{ + private final double dt; + + private final YoBoolean hasBeenInitialized; + + private final YoDouble smoothedRate; + private final YoDouble smoothedAcceleration; + private final YoDouble smoothedJerk; + + private final YoDouble positionGain; + private final YoDouble velocityGain; + private final YoDouble accelerationGain; + + private final YoDouble maximumJerk; + private final YoDouble maximumAcceleration; + + private final YoDouble inputPosition; + private final YoDouble inputVelocity; + private final YoDouble inputAcceleration; + + public JerkLimitedYoDouble(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, double dt) + { + this(name, registry, maxAcceleration, maxJerk, null, null, null, dt); + } + + public JerkLimitedYoDouble(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, + YoDouble inputPosition, double dt) + { + this(name, registry, maxAcceleration, maxJerk, inputPosition, null, null, dt); + } + + public JerkLimitedYoDouble(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, + YoDouble inputPosition, YoDouble inputVelocity, double dt) + { + this(name, registry, maxAcceleration, maxJerk, inputPosition, inputVelocity, null, dt); + } + + public JerkLimitedYoDouble(String name, YoRegistry registry, YoDouble maxAcceleration, YoDouble maxJerk, + YoDouble inputPosition, YoDouble inputVelocity, YoDouble inputAcceleration, double dt) + { + super(name, registry); + + this.inputPosition = inputPosition; + this.inputVelocity = inputVelocity; + this.inputAcceleration = inputAcceleration; + + this.maximumJerk = maxJerk; + this.maximumAcceleration = maxAcceleration; + + this.dt = dt; + + hasBeenInitialized = new YoBoolean(name + "HasBeenInitialized", registry); + + smoothedRate = new YoDouble(name + "SmoothedRate", registry); + smoothedAcceleration = new YoDouble(name + "SmoothedAcceleration", registry); + smoothedJerk = new YoDouble(name + "SmoothedJerk", registry); + + positionGain = new YoDouble(name + "PositionGain", registry); + velocityGain = new YoDouble(name + "VelocityGain", registry); + accelerationGain = new YoDouble(name + "AccelerationGain", registry); + + double w0 = 2.0 * Math.PI * 16.0; + double w1 = 2.0 * Math.PI * 16.0; + double zeta = 1.0; + + setGainsByPolePlacement(w0, w1, zeta); + hasBeenInitialized.set(false); + } + + public void setMaximumAcceleration(double maximumAcceleration) + { + this.maximumAcceleration.set(maximumAcceleration); + } + + public void setMaximumJerk(double maximumJerk) + { + this.maximumJerk.set(maximumJerk); + } + + public void setGainsByPolePlacement(double w0, double w1, double zeta) + { + positionGain.set(w0 * w1 * w1); + velocityGain.set(w1 * w1 + 2.0 * zeta * w1 * w0); + accelerationGain.set(w0 + 2.0 * zeta * w1); + } + + public void update() + { + double inputPosition = this.inputPosition == null ? 0.0 : this.inputPosition.getDoubleValue(); + double inputVelocity = this.inputVelocity == null ? 0.0 : this.inputVelocity.getDoubleValue(); + double inputAcceleration = this.inputAcceleration == null ? 0.0 : this.inputAcceleration.getDoubleValue(); + + update(inputPosition, inputVelocity, inputAcceleration); + } + + public void update(double inputPosition) + { + update(inputPosition, 0.0, 0.0); + } + + public void update(double inputPosition, double inputVelocity) + { + update(inputPosition, inputVelocity, 0.0); + } + + public void update(double inputPosition, double inputVelocity, double inputAcceleration) + { + if (!hasBeenInitialized.getBooleanValue()) + initialize(inputPosition, inputVelocity, inputAcceleration); + + double positionError = inputPosition - this.getDoubleValue(); + double velocityError = inputVelocity - smoothedRate.getDoubleValue(); + double accelerationError = inputAcceleration - smoothedAcceleration.getDoubleValue(); + double jerk = accelerationGain.getDoubleValue() * accelerationError + velocityGain.getDoubleValue() * velocityError + positionGain.getDoubleValue() + * positionError; + jerk = MathTools.clamp(jerk, -maximumJerk.getDoubleValue(), maximumJerk.getDoubleValue()); + + smoothedJerk.set(jerk); + smoothedAcceleration.add(smoothedJerk.getDoubleValue() * dt); + smoothedAcceleration.set(MathTools.clamp(smoothedAcceleration.getDoubleValue(), maximumJerk.getDoubleValue())); + smoothedRate.add(smoothedAcceleration.getDoubleValue() * dt); + this.add(smoothedRate.getDoubleValue() * dt); + } + + public void initialize(double inputPosition, double inputVelocity, double inputAcceleration) + { + this.set(inputPosition); + smoothedRate.set(inputVelocity); + smoothedAcceleration.set(inputAcceleration); + smoothedJerk.set(0.0); + + this.hasBeenInitialized.set(true); + } + + public void reset() + { + this.hasBeenInitialized.set(false); + smoothedRate.set(0.0); + smoothedAcceleration.set(0.0); + smoothedJerk.set(0.0); + } + +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java new file mode 100644 index 00000000..bad8f2db --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java @@ -0,0 +1,114 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +/** + * @author thutcheson + *

+ *

+ *

+ *

+ * A BetaFilteredYoVariable is a filtered version of an input YoVar. + * This is a moving average filter. + * Either a YoVariable holding the unfiltered val is passed in to the + * constructor and update() is called every tick, or update(double) is + * called every tick. The BetaFilteredYoVariable updates it's val + * with the current filtered version using + *

+ * 
+ *            filtered_{n} = (raw_{0} + ... + raw_{n-1} + raw_{n}) / n
+ *
+ */ +public class MovingAverageYoDouble extends YoDouble +{ + private int beta; + private int index = 0; + @SuppressWarnings("unused") + private final YoDouble betaVariable; + + private final YoDouble position; + + private final double[] raw; + private boolean bufferFull = false; + + private final YoBoolean hasBeenCalled; + + public MovingAverageYoDouble(String name, YoRegistry registry, int beta) + { + this(name, "", registry, beta, null); + } + + public MovingAverageYoDouble(String name, String description, YoRegistry registry, int beta) + { + this(name, description, registry, beta, null); + } + + public MovingAverageYoDouble(String name, YoRegistry registry, int beta, YoDouble positionVariable) + { + this(name, "", registry, beta, positionVariable); + } + + public MovingAverageYoDouble(String name, String description, YoRegistry registry, int beta, YoDouble positionVariable) + { + super(name, description, registry); + this.hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(name, "", registry); + + this.beta = beta; + this.position = positionVariable; + this.betaVariable = null; + + raw = new double[beta]; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + bufferFull = false; + index = 0; + + for (int i = 0; i < beta; i++) + { + set(0.0); + } + } + + public void update() + { + if (position == null) + { + throw new NullPointerException("BetaFilteredYoVariable must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(position.getDoubleValue()); + } + + public void update(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + set(currentPosition, false); + } + + raw[index++] = currentPosition; + if (index == beta) + { + index = 0; + bufferFull = true; + } + + final int size = bufferFull ? beta : index; + double value = 0.0; + for (int i = 0; i < size; i++) + { + value += raw[i]; + } + + set(value / size); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java new file mode 100644 index 00000000..d3182ab8 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java @@ -0,0 +1,10 @@ +package us.ihmc.yoVariables.filters; + +public interface ProcessingYoVariable +{ + public abstract void update(); + + public default void reset() + { + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java new file mode 100644 index 00000000..bcc4b17f --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java @@ -0,0 +1,87 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +public class RateLimitedYoVariable extends YoDouble +{ + private final DoubleProvider maxRateVariable; + + private final DoubleProvider unlimitedPosition; + private final YoBoolean limited; + + private final double dt; + + private final YoBoolean hasBeenCalled; + + public RateLimitedYoVariable(String name, YoRegistry registry, double maxRate, double dt) + { + this(name, registry, maxRate, null, dt); + } + + public RateLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRateVariable, double dt) + { + this(name, registry, maxRateVariable, null, dt); + } + + public RateLimitedYoVariable(String name, YoRegistry registry, double maxRate, DoubleProvider positionVariable, double dt) + { + this(name, registry, VariableTools.createMaxRateYoDouble(name, "", maxRate, registry), positionVariable, dt); + } + + public RateLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRateVariable, DoubleProvider unlimitedPosition, double dt) + { + super(name, registry); + + this.hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(name, "", registry); + this.limited = VariableTools.createLimitedCalledYoBoolean(name, "", registry); + + this.unlimitedPosition = unlimitedPosition; + this.maxRateVariable = maxRateVariable; + + this.dt = dt; + + reset(); + } + + public void reset() + { + hasBeenCalled.set(false); + } + + public void update() + { + if (unlimitedPosition == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "position variable to call update(), otherwise use update(double)"); + } + + update(unlimitedPosition.getValue()); + } + + public void update(double currentPosition) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + set(currentPosition); + } + + if (maxRateVariable.getValue() < 0) + throw new RuntimeException("The maxRate parameter in the RateLimitedYoVariable cannot be negative."); + + double difference = currentPosition - getDoubleValue(); + if (Math.abs(difference) > maxRateVariable.getValue() * dt) + { + difference = Math.signum(difference) * maxRateVariable.getValue() * dt; + this.limited.set(true); + } + else + this.limited.set(false); + + set(getDoubleValue() + difference); + } +} \ No newline at end of file diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java new file mode 100644 index 00000000..25c05db6 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java @@ -0,0 +1,52 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoInteger; + +public class RunningAverageYoDouble extends YoDouble +{ + private final YoInteger sampleSize; + private final DoubleProvider dataSource; + + public RunningAverageYoDouble(String name, YoRegistry registry) + { + this(name, null, registry); + } + + public RunningAverageYoDouble(String name, DoubleProvider dataSource, YoRegistry registry) + { + super(name, registry); + + this.dataSource = dataSource; + sampleSize = new YoInteger(name + "SampleSize", registry); + } + + public void update() + { + if (dataSource == null) + { + throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + + "dataSource variable to call update(), otherwise use update(double)"); + } + + update(dataSource.getValue()); + } + + public void update(double dataSource) + { + sampleSize.increment(); + add((dataSource - getValue()) / sampleSize.getValue()); + } + + public void reset() + { + sampleSize.set(0); + } + + public int getSampleSize() + { + return sampleSize.getValue(); + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilterType.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilterType.java new file mode 100644 index 00000000..d524c37e --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilterType.java @@ -0,0 +1,6 @@ +package us.ihmc.yoVariables.filters; + +public enum SecondOrderFilterType +{ + LOW_PASS, NOTCH, BAND, HIGH_PASS +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java new file mode 100644 index 00000000..28893835 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java @@ -0,0 +1,204 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; + +/** + * A discrete-time second order filter using the bilinear transform + *

+ * ---------------------------------------------------------------- + * LOW PASS: + *

+ * Y(s) omega^2 + * ---- = ---------------------------------- + * X(s) s^2 + 2 * xi * omega * s + omega^2 + *

+ * ---------------------------------------------------------------- + * NOTCH: + *

+ * Y(s) s^2 + omega^2 + * ---- = ---------------------------------- + * X(s) s^2 + 2 * xi * omega * s + omega^2 + *

+ * ---------------------------------------------------------------- + * HIGH PASS: + *

+ * Y(s) s^2 + * ---- = ---------------------------------- + * X(s) s^2 + 2 * xi * omega * s + omega^2 + *

+ * ----------------------------------------------------------------- + *

+ * omega = 2 * PI * naturalFrequencyInHz + * xi = dampingRatio + */ +public class SecondOrderFilteredYoDouble extends YoDouble implements ProcessingYoVariable +{ + private final double dt; + private final SecondOrderFilteredYoVariableParameters parameters; + protected final YoBoolean hasBeenCalled; + private final YoDouble inputVariable; + private final YoDouble[] input; + private final YoDouble[] output; + private final double a[]; + private final double b[]; + + public SecondOrderFilteredYoDouble(String name, YoRegistry registry, double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType) + { + this(name, registry, dt, new SecondOrderFilteredYoVariableParameters(name, registry, naturalFrequencyInHz, dampingRatio, filterType), null); + } + + public SecondOrderFilteredYoDouble(String name, YoRegistry registry, double dt, SecondOrderFilteredYoVariableParameters parameters) + { + this(name, registry, dt, parameters, null); + } + + public SecondOrderFilteredYoDouble(String name, + YoRegistry registry, + double dt, + double naturalFrequencyInHz, + double dampingRatio, + SecondOrderFilterType filterType, + YoDouble inputVariable) + { + this(name, registry, dt, new SecondOrderFilteredYoVariableParameters(name, registry, naturalFrequencyInHz, dampingRatio, filterType), inputVariable); + } + + public SecondOrderFilteredYoDouble(String name, YoRegistry registry, double dt, SecondOrderFilteredYoVariableParameters parameters, YoDouble inputVariable) + { + super(name, registry); + this.dt = dt; + this.parameters = parameters; + this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry); + this.inputVariable = inputVariable; + this.input = new YoDouble[3]; + this.output = new YoDouble[3]; + this.a = new double[3]; + this.b = new double[3]; + for (int i = 0; i < 3; i++) + { + this.input[i] = new YoDouble(name + "input" + i, registry); + this.output[i] = new YoDouble(name + "output" + i, registry); + } + reset(); + } + + @Override + public void reset() + { + hasBeenCalled.set(false); + computeCoefficients(); + } + + @Override + public void update() + { + if (inputVariable == null) + { + throw new NullPointerException( + "SecondOrderFilteredYoVariable must be constructed with a non null position variable to call update(), otherwise use update(double)"); + } + + update(inputVariable.getDoubleValue()); + } + + public void update(double currentInputValue) + { + if (!hasBeenCalled.getBooleanValue()) + { + hasBeenCalled.set(true); + set(currentInputValue); + for (int i = 0; i < 3; i++) + { + input[i].set(currentInputValue); + output[i].set(currentInputValue); + } + return; + } + + for (int i = 2; i > 0; i--) + { + input[i].set(input[i - 1].getDoubleValue()); + output[i].set(output[i - 1].getDoubleValue()); + } + input[0].set(currentInputValue); + + double currentOutputValue = 0.0; + currentOutputValue += b[2] * input[2].getDoubleValue(); + currentOutputValue += b[1] * input[1].getDoubleValue(); + currentOutputValue += b[0] * input[0].getDoubleValue(); + currentOutputValue -= a[2] * output[2].getDoubleValue(); + currentOutputValue -= a[1] * output[1].getDoubleValue(); + currentOutputValue /= a[0]; + output[0].set(currentOutputValue); + + set(currentOutputValue); + } + + public void setNaturalFrequencyInHz(double naturalFrequencyInHz) + { + parameters.getNaturalFrequencyInHz().set(Math.min(Math.max(naturalFrequencyInHz, 0.0), 1.0 / (2.0 * dt))); + computeCoefficients(); + } + + public void setDampingRatio(double dampingRatio) + { + parameters.getDampingRatio().set(Math.max(dampingRatio, 0.0)); + computeCoefficients(); + } + + public boolean getHasBeenCalled() + { + return hasBeenCalled.getBooleanValue(); + } + + public void getFilterCoefficients(double[] b, double[] a) + { + if (b.length < 3) + throw new RuntimeException("b must be of length 3 or greater"); + + if (a.length < 3) + throw new RuntimeException("a must be of length 3 or greater"); + + for (int i = 0; i < 3; i++) + b[i] = this.b[i]; + for (int i = 3; i < b.length; i++) + b[i] = 0.0; + for (int i = 0; i < 3; i++) + a[i] = this.a[i]; + for (int i = 3; i < a.length; i++) + a[i] = 0.0; + } + + private void computeCoefficients() + { + double omega = 2 * Math.PI * parameters.getNaturalFrequencyInHz().getDoubleValue(); + double xi = parameters.getDampingRatio().getDoubleValue(); + + switch (parameters.getFilterType()) + { + case LOW_PASS: + b[0] = omega * omega; + b[1] = 2.0 * omega * omega; + b[2] = omega * omega; + break; + case NOTCH: + b[0] = 4.0 / (dt * dt) + omega * omega; + b[1] = 2.0 * omega * omega - 8.0 / (dt * dt); + b[2] = 4.0 / (dt * dt) + omega * omega; + break; + case HIGH_PASS: + b[0] = 4.0 / (dt * dt); + b[1] = -8.0 / (dt * dt); + b[2] = 4.0 / (dt * dt); + break; + case BAND: + throw new IllegalArgumentException("Band pass filters are not established for the second order filter yo variable."); + } + + a[0] = 4.0 / (dt * dt) + 4.0 / dt * xi * omega + omega * omega; + a[1] = 2.0 * omega * omega - 8.0 / (dt * dt); + a[2] = 4.0 / (dt * dt) - 4.0 / dt * xi * omega + omega * omega; + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java new file mode 100644 index 00000000..68bf4f78 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java @@ -0,0 +1,36 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class SecondOrderFilteredYoVariableParameters +{ + private final YoDouble naturalFrequencyInHz; + private final YoDouble dampingRatio; + private final SecondOrderFilterType filterType; + + public SecondOrderFilteredYoVariableParameters(String name, YoRegistry registry, double naturalFrequencyInHz, double dampingRatio, + SecondOrderFilterType filterType) + { + this.naturalFrequencyInHz = new YoDouble(name + "NaturalFrequency", registry); + this.naturalFrequencyInHz.set(naturalFrequencyInHz); + this.dampingRatio = new YoDouble(name + "DampingRatio", registry); + this.dampingRatio.set(dampingRatio); + this.filterType = filterType; + } + + public YoDouble getNaturalFrequencyInHz() + { + return naturalFrequencyInHz; + } + + public YoDouble getDampingRatio() + { + return dampingRatio; + } + + public SecondOrderFilterType getFilterType() + { + return filterType; + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java new file mode 100644 index 00000000..2628b7d9 --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java @@ -0,0 +1,87 @@ +package us.ihmc.yoVariables.filters; + +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoInteger; + +/** + * Filter the given yoVariable using a moving average filter. This class is NOT REWINDABLE! + */ +public class SimpleMovingAverageFilteredYoVariable extends YoDouble +{ + private final YoInteger windowSize; + private final YoDouble yoVariableToFilter; + + private final DMatrixRMaj previousUpdateValues = new DMatrixRMaj(0, 0); + private int bufferPosition = 0; + + private boolean bufferHasBeenFilled = false; + + public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoRegistry registry) + { + this(name, windowSize, null, registry); + } + + public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoDouble yoVariableToFilter, YoRegistry registry) + { + super(name, registry); + + this.yoVariableToFilter = yoVariableToFilter; + this.windowSize = new YoInteger(name + "WindowSize", registry); + this.windowSize.set(windowSize); + + previousUpdateValues.reshape(windowSize, 1); + CommonOps_DDRM.fill(previousUpdateValues, 0.0); + } + + public void setWindowSize(int windowSize) + { + this.windowSize.set(windowSize); + reset(); + } + + public void update() + { + update(yoVariableToFilter.getDoubleValue()); + } + + public void update(double value) + { + if (previousUpdateValues.getNumRows() != windowSize.getIntegerValue()) + { + reset(); + } + previousUpdateValues.set(bufferPosition, 0, value); + + bufferPosition++; + + if (bufferPosition >= windowSize.getIntegerValue()) + { + bufferPosition = 0; + bufferHasBeenFilled = true; + } + + double average = 0; + for (int i = 0; i < windowSize.getIntegerValue(); i++) + { + average += previousUpdateValues.get(i, 0); + } + + this.set(average / ((double) windowSize.getIntegerValue())); + } + + public void reset() + { + bufferPosition = 0; + bufferHasBeenFilled = false; + previousUpdateValues.reshape(windowSize.getIntegerValue(), 1); + } + + public boolean getHasBufferWindowFilled() + { + return bufferHasBeenFilled; + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/VariableTools.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/VariableTools.java new file mode 100644 index 00000000..d798ac1a --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/VariableTools.java @@ -0,0 +1,48 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoInteger; + +public class VariableTools +{ + public static YoBoolean createHasBeenCalledYoBoolean(String namePrefix, String nameSuffix, YoRegistry registry) + { + return new YoBoolean(namePrefix + "HasBeenCalled" + nameSuffix, registry); + } + + public static YoBoolean createLimitedCalledYoBoolean(String namePrefix, String nameSuffix, YoRegistry registry) + { + return new YoBoolean(namePrefix + "Limited" + nameSuffix, registry); + } + + public static DoubleProvider createMaxRateYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxRate" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } + + public static DoubleProvider createAlphaYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "AlphaVariable" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } + + public static YoInteger createWindowSizeYoInteger(String namePrefix, String nameSuffix, int initialValue, YoRegistry registry) + { + YoInteger windowSize = new YoInteger(namePrefix + "WindowSize" + nameSuffix, registry); + windowSize.set(initialValue); + return windowSize; + } + + public static DoubleProvider createMaxAccelerationYoDouble(String namePrefix, String nameSuffix, double initialValue, YoRegistry registry) + { + YoDouble maxRate = new YoDouble(namePrefix + "MaxAcceleration" + nameSuffix, registry); + maxRate.set(initialValue); + return maxRate; + } +} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/YoMatrix.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/YoMatrix.java new file mode 100644 index 00000000..e640d87b --- /dev/null +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/YoMatrix.java @@ -0,0 +1,352 @@ +package us.ihmc.yoVariables.filters; + +import org.ejml.data.*; +import org.ejml.ops.MatrixIO; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.YoInteger; + +import java.io.ByteArrayOutputStream; +import java.io.PrintStream; + +/** + * YoMatrix. Object for holding a matrix of YoVariables so that Matrices can be rewound. Has a + * maximum number of rows and columns and an actual number of rows and columns. If you set with a + * smaller matrix, then the actual size will be the size of the passed in matrix. extra entries will + * be set to NaN. If you get the contents the matrix you pack must be the correct size. + * + * @author JerryPratt + */ +public class YoMatrix implements DMatrix, ReshapeMatrix +{ + // TODO: eventually consolidate YoMatrix implementations + + private static final long serialVersionUID = 2156411740647948028L; + + private final int maxNumberOfRows, maxNumberOfColumns; + + private final YoInteger numberOfRows, numberOfColumns; + private final YoDouble[][] variables; + + public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) + { + this(name, null, maxNumberOfRows, maxNumberOfColumns, null, null, registry); + } + + public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, YoRegistry registry) + { + this(name, null, maxNumberOfRows, maxNumberOfColumns, rowNames, null, registry); + } + + public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) + { + this(name, null, maxNumberOfRows, maxNumberOfColumns, rowNames, columnNames, registry); + } + + public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) + { + this(name, description, maxNumberOfRows, maxNumberOfColumns, null, null, registry); + } + + public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, YoRegistry registry) + { + this(name, description, maxNumberOfRows, maxNumberOfColumns, rowNames, null, registry); + } + + public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) + { + this.maxNumberOfRows = maxNumberOfRows; + this.maxNumberOfColumns = maxNumberOfColumns; + + this.numberOfRows = new YoInteger(name + "NumRows", registry); + this.numberOfColumns = new YoInteger(name + "NumCols", registry); + + this.numberOfRows.set(maxNumberOfRows); + this.numberOfColumns.set(maxNumberOfColumns); + + variables = new YoDouble[maxNumberOfRows][maxNumberOfColumns]; + + for (int row = 0; row < maxNumberOfRows; row++) + { + for (int column = 0; column < maxNumberOfColumns; column++) + { + switch (checkNames(rowNames, columnNames)) + { + case NONE: + { + variables[row][column] = new YoDouble(getFieldName(name, row, column), description, registry); // names are simply the row and column indices + variables[row][column].setToNaN(); + break; + } + case ROWS: + { + if (maxNumberOfColumns > 1) + throw new IllegalArgumentException( + "The YoMatrix must be a column vector if only row names are provided, else unique names cannot be generated."); + + variables[row][column] = new YoDouble(getFieldName(name, rowNames[row], ""), description, registry); // names are the row names, no column identifier + variables[row][column].setToNaN(); + break; + } + case ROWS_AND_COLUMNS: + { + variables[row][column] = new YoDouble(getFieldName(name, rowNames[row], columnNames[column]), description, registry); // names are the row and column names + variables[row][column].setToNaN(); + break; + } + } + } + } + } + + public static String getFieldName(String prefix, int row, int column) + { + return getFieldName(prefix, "_" + row, "_" + column); + } + + public static String getFieldName(String prefix, String rowName, String columName) + { + return prefix + rowName + columName; + } + + private enum NamesProvided + { + NONE, ROWS, ROWS_AND_COLUMNS + } + + private NamesProvided checkNames(String[] rowNames, String[] columnNames) + { + if (rowNames == null && columnNames == null) + return NamesProvided.NONE; + else if (rowNames != null && columnNames == null) + return NamesProvided.ROWS; + else + return NamesProvided.ROWS_AND_COLUMNS; + } + + @Override + public double get(int row, int col) + { + if (col < 0 || col >= getNumCols() || row < 0 || row >= getNumRows()) + throw new IllegalArgumentException("Specified element is out of bounds: (" + row + " , " + col + ")"); + return unsafe_get(row, col); + } + + @Override + public double unsafe_get(int row, int col) + { + return variables[row][col].getValue(); + } + + @Override + public void set(int row, int col, double val) + { + if (col < 0 || col >= getNumCols() || row < 0 || row >= getNumRows()) + throw new IllegalArgumentException("Specified element is out of bounds: (" + row + " , " + col + ")"); + unsafe_set(row, col, val); + } + + @Override + public void unsafe_set(int row, int col, double val) + { + unsafe_set(row, col, val, true); + } + + private void unsafe_set(int row, int col, double val, boolean notifyListeners) + { + variables[row][col].set(val, notifyListeners); + } + + @Override + public int getNumElements() + { + return numberOfRows.getValue() * numberOfColumns.getValue(); + } + + @Override + public int getNumRows() + { + return numberOfRows.getValue(); + } + + @Override + public int getNumCols() + { + return numberOfColumns.getValue(); + } + + @Override + public void zero() + { + for (int row = 0; row < getNumRows(); row++) + { + for (int col = 0; col < getNumCols(); col++) + { + variables[row][col].set(0.0); + } + } + } + + @Override + public T copy() + { + throw new UnsupportedOperationException(); + } + + @Override + public T createLike() + { + throw new UnsupportedOperationException(); + } + + @Override + public T create(int numRows, int numCols) + { + throw new UnsupportedOperationException(); + } + + @Override + public void set(Matrix original) + { + if (original instanceof DMatrix otherMatrix) + { + reshape(otherMatrix.getNumRows(), otherMatrix.getNumCols()); + for (int row = 0; row < getNumRows(); row++) + { + for (int col = 0; col < getNumCols(); col++) + { + unsafe_set(row, col, otherMatrix.unsafe_get(row, col), false); + } + } + } + } + + @Override + public void print() + { + MatrixIO.printFancy(System.out, this, MatrixIO.DEFAULT_LENGTH); + } + + @Override + public void print(String format) + { + MatrixIO.print(System.out, this, format); + } + + @Override + public MatrixType getType() + { + return MatrixType.UNSPECIFIED; + } + + @Override + public void reshape(int numRows, int numCols) + { + if (numRows > maxNumberOfRows) + throw new IllegalArgumentException("Too many rows. Expected less or equal to " + maxNumberOfRows + ", was " + numRows); + else if (numCols > maxNumberOfColumns) + throw new IllegalArgumentException("Too many columns. Expected less or equal to " + maxNumberOfColumns + ", was " + numCols); + else if (numRows < 0 || numCols < 0) + throw new IllegalArgumentException("Cannot reshape with a negative number of rows or columns."); + + numberOfRows.set(numRows); + numberOfColumns.set(numCols); + + for (int row = 0; row < numRows; row++) + { + for (int col = numCols; col < maxNumberOfColumns; col++) + { + unsafe_set(row, col, Double.NaN, false); + } + } + + for (int row = numRows; row < maxNumberOfRows; row++) + { + for (int col = 0; col < maxNumberOfColumns; col++) + { + unsafe_set(row, col, Double.NaN, false); + } + } + } + + public void set(DMatrix matrix) + { + int numRows = matrix.getNumRows(); + int numCols = matrix.getNumCols(); + + if (((numRows > maxNumberOfRows) || (numCols > maxNumberOfColumns)) && (numRows > 0) && (numCols > 0)) + throw new RuntimeException("Not enough rows or columns. matrix to set is " + matrix.getNumRows() + " by " + matrix.getNumCols()); + + this.numberOfRows.set(numRows); + this.numberOfColumns.set(numCols); + + for (int row = 0; row < maxNumberOfRows; row++) + { + for (int column = 0; column < maxNumberOfColumns; column++) + { + double value; + if ((row < numRows) && (column < numCols)) + { + value = matrix.unsafe_get(row, column); + } + else + { + value = Double.NaN; + } + unsafe_set(row, column, value, false); + + } + } + } + + public void getAndReshape(DMatrixRMaj matrixToPack) + { + matrixToPack.reshape(getNumRows(), getNumCols()); + get(matrixToPack); + } + + public void get(DMatrix matrixToPack) + { + int numRows = matrixToPack.getNumRows(); + int numCols = matrixToPack.getNumCols(); + + if (((numRows > maxNumberOfRows) || (numCols > maxNumberOfColumns)) && (numRows > 0) && (numCols > 0)) + throw new RuntimeException("Not enough rows or columns. matrixToPack is " + matrixToPack.getNumRows() + " by " + matrixToPack.getNumCols()); + if ((numRows != this.numberOfRows.getIntegerValue()) || (numCols != this.numberOfColumns.getIntegerValue())) + throw new RuntimeException("Numer of rows and columns must be the same. Call getAndReshape() if you want to reshape the matrixToPack"); + + for (int row = 0; row < numRows; row++) + { + for (int column = 0; column < numCols; column++) + { + matrixToPack.unsafe_set(row, column, variables[row][column].getDoubleValue()); + } + } + } + + public void setToNaN(int numberOfRows, int numberOfColumns) + { + reshape(numberOfRows, numberOfColumns); + for (int row = 0; row < numberOfRows; row++) + { + for (int col = 0; col < numberOfColumns; col++) + { + unsafe_set(row, col, Double.NaN, false); + } + } + } + + public YoDouble getYoDouble(int row, int col) + { + return variables[row][col]; + } + + @Override + public String toString() + { + ByteArrayOutputStream stream = new ByteArrayOutputStream(); + MatrixIO.print(new PrintStream(stream), this); + + return stream.toString(); + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java new file mode 100644 index 00000000..91280c6d --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java @@ -0,0 +1,152 @@ +package us.ihmc.yoVariables.euclid.filters; + +import org.junit.jupiter.api.Test; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.euclid.yawPitchRoll.YawPitchRoll; + +import java.util.Random; + +public class AlphaFilteredRigidBodyTransformTest +{ + private static final double EPSILON = 1.0e-15; + + @Test + public void testRegression() + { + Random random = new Random(3453456); + + double alpha = random.nextDouble(); + + AlphaFilteredRigidBodyTransform filteredRigidBodyTransform = new AlphaFilteredRigidBodyTransform(); + filteredRigidBodyTransform.setAlpha(0.9); + + RigidBodyTransform unfilteredRigidBodyTransform = new RigidBodyTransform(); + + unfilteredRigidBodyTransform.getTranslation().add(new Point3D(0.1, 0.1, 0.1)); + unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, 0.1, 0.1)); + + filteredRigidBodyTransform.update(unfilteredRigidBodyTransform); + + System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform)); + + EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString(""" + 0.9900332889206207 -0.0894177463594257 0.1088051169064211 | 0.1000000000000000 + 0.0993346653975306 0.9910282997404070 -0.0894177463594257 | 0.1000000000000000 + -0.0998334166468282 0.0993346653975306 0.9900332889206207 | 0.1000000000000000 + 0.0000000000000000 0.0000000000000000 0.0000000000000000 | 1.0000000000000000 + """), filteredRigidBodyTransform, EPSILON); + unfilteredRigidBodyTransform.getTranslation().add(new Point3D(0.1, 0.1, 0.1)); + unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, 0.1, 0.1)); + + filteredRigidBodyTransform.update(unfilteredRigidBodyTransform); + + System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform)); + + EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString(""" + 0.9879463963435010 -0.0977116814683914 0.1200597570233632 | 0.1100000000000000 + 0.1097050673132180 0.9891497487794362 -0.0977116814683914 | 0.1100000000000000 + -0.1092095058027995 0.1097050673132180 0.9879463963435011 | 0.1100000000000000 + 0.0000000000000000 0.0000000000000000 0.0000000000000000 | 1.0000000000000000 + """), filteredRigidBodyTransform, EPSILON); + unfilteredRigidBodyTransform.getTranslation().add(new Point3D(0.1, 0.1, 0.1)); + unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, 0.1, 0.1)); + + filteredRigidBodyTransform.update(unfilteredRigidBodyTransform); + + System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform)); + + EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString(""" + 0.9834409873207434 -0.1131163390455875 0.1415927904185955 | 0.1290000000000000 + 0.1295926256343390 0.9850941301328119 -0.1131163390455875 | 0.1290000000000000 + -0.1266869205514076 0.1295926256343390 0.9834409873207435 | 0.1290000000000000 + 0.0000000000000000 0.0000000000000000 0.0000000000000000 | 1.0000000000000000 + """), filteredRigidBodyTransform, EPSILON); + unfilteredRigidBodyTransform.getTranslation().add(new Point3D(0.1, 0.1, 0.1)); + unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, 0.1, 0.1)); + + filteredRigidBodyTransform.update(unfilteredRigidBodyTransform); + + System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform)); + + EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString(""" + 0.9757980801204758 -0.1342587634073139 0.1726055945835227 | 0.1561000000000000 + 0.1583397744951649 0.9782142404714618 -0.1342587634073140 | 0.1561000000000000 + -0.1508198350549846 0.1583397744951649 0.9757980801204760 | 0.1561000000000000 + 0.0000000000000000 0.0000000000000000 0.0000000000000000 | 1.0000000000000000 + """), filteredRigidBodyTransform, EPSILON); + unfilteredRigidBodyTransform.getTranslation().add(new Point3D(0.1, 0.1, 0.1)); + unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, 0.1, 0.1)); + + filteredRigidBodyTransform.update(unfilteredRigidBodyTransform); + + System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform)); + + EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString(""" + 0.9640634283712346 -0.1596208716326060 0.2123743944460991 | 0.1904900000000000 + 0.1953779100890390 0.9676510990995081 -0.1596208716326060 | 0.1904900000000000 + -0.1800254935456074 0.1953779100890391 0.9640634283712346 | 0.1904900000000000 + 0.0000000000000000 0.0000000000000000 0.0000000000000000 | 1.0000000000000000 + """), filteredRigidBodyTransform, EPSILON); + } + + @Test + public void testReset() + { + Random random = new Random(3453456); + + double alpha = random.nextDouble(); + + AlphaFilteredRigidBodyTransform filteredRigidBodyTransform = new AlphaFilteredRigidBodyTransform(); + filteredRigidBodyTransform.setAlpha(0.9); + + RigidBodyTransform unfilteredRigidBodyTransform = new RigidBodyTransform(); + + unfilteredRigidBodyTransform.getTranslation().add(new Point3D(-0.1, 0.1, -0.1)); + unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, -0.1, 0.1)); + filteredRigidBodyTransform.update(unfilteredRigidBodyTransform); + System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform)); + EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString(""" + 0.990033288920620700 -0.109251584435635540 -0.088871694747662740 | -0.100000000000000000 + 0.099334665397530610 0.989038278100834400 -0.109251584435635540 | 0.100000000000000000 + 0.099833416646828150 0.099334665397530610 0.990033288920620700 | -0.100000000000000000 + 0.000000000000000000 0.000000000000000000 0.000000000000000000 | 1.000000000000000000 + """), filteredRigidBodyTransform, EPSILON); + + unfilteredRigidBodyTransform.getTranslation().add(new Point3D(-0.1, 0.1, -0.1)); + unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, -0.1, 0.1)); + filteredRigidBodyTransform.update(unfilteredRigidBodyTransform); + System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform)); + EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString(""" + 0.987946818678216400 -0.120594329665051940 -0.097046850109766770 | -0.110000000000000010 + 0.108601364045026250 0.986743508405399000 -0.120594329665051990 | 0.110000000000000010 + 0.110303341704367470 0.108601364045026240 0.987946818678216500 | -0.110000000000000010 + 0.000000000000000000 0.000000000000000000 0.000000000000000000 | 1.000000000000000000 + """), filteredRigidBodyTransform, EPSILON); + + filteredRigidBodyTransform.reset(); + + unfilteredRigidBodyTransform.getTranslation().add(new Point3D(-0.1, 0.1, -0.1)); + unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, -0.1, 0.1)); + filteredRigidBodyTransform.update(unfilteredRigidBodyTransform); + System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform)); + EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString(""" + 0.912137624808415300 -0.343802033741961170 -0.223170596189895150 | -0.300000000000000040 + 0.256378604455114070 0.903366023698333700 -0.343802033741961170 | 0.300000000000000040 + 0.319804572491560860 0.256378604455114070 0.912137624808415400 | -0.300000000000000040 + 0.000000000000000000 0.000000000000000000 0.000000000000000000 | 1.000000000000000000 + """), filteredRigidBodyTransform, EPSILON); + + unfilteredRigidBodyTransform.getTranslation().add(new Point3D(-0.1, 0.1, -0.1)); + unfilteredRigidBodyTransform.getRotation().append(new YawPitchRoll(0.1, -0.1, 0.1)); + filteredRigidBodyTransform.update(unfilteredRigidBodyTransform); + System.out.println(EuclidCoreTestMissingTools.toStringFullPrecision(filteredRigidBodyTransform)); + EuclidCoreTestTools.assertEquals(EuclidCoreTestMissingTools.newRigidBodyTransformFromString(""" + 0.906330827001449200 -0.355699718189048300 -0.228127469865204740 | -0.310000000000000050 + 0.262498500897342700 0.896979513426520900 -0.355699718189048330 | 0.310000000000000050 + 0.331147956438683330 0.262498500897342700 0.906330827001449300 | -0.310000000000000050 + 0.000000000000000000 0.000000000000000000 0.000000000000000000 | 1.000000000000000000 + """), filteredRigidBodyTransform, EPSILON); + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java new file mode 100644 index 00000000..80aab55a --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java @@ -0,0 +1,106 @@ +package us.ihmc.yoVariables.euclid.filters; + +import org.junit.jupiter.api.Test; +import us.ihmc.commons.Epsilons; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.euclid.tuple3D.Tuple3DBasicsTest; + +import java.util.Random; + +import static org.junit.jupiter.api.Assertions.*; + +public class AlphaFilteredTuple3DTest extends Tuple3DBasicsTest +{ + private double alpha; + + @Test + public void testFirstSet() + { + double alpha = 0.0; + AlphaFilteredTuple3D tuple = new AlphaFilteredTuple3D(() -> alpha); + tuple.set(0.0, 1.0, 2.0); + + assertEquals(0.0, tuple.getX(), getEpsilon()); + assertEquals(1.0, tuple.getY(), getEpsilon()); + assertEquals(2.0, tuple.getZ(), getEpsilon()); + } + + @Test + public void testFilteredSetters() + { + Random random = new Random(12951L); + for (int i = 0; i < 1000; ++i) + { + alpha = random.nextDouble(0.0, 1.0); + AlphaFilteredTuple3D tuple = createRandomTuple(random); + + double originalX = tuple.getX(); + double originalY = tuple.getY(); + double originalZ = tuple.getZ(); + + Point3D point = new Point3D(createRandomTuple(random)); + + tuple.setX(point.getX()); + tuple.setY(point.getY()); + tuple.setZ(point.getZ()); + + double expectedX = (1.0 - alpha) * point.getX() + alpha * originalX; + double expectedY = (1.0 - alpha) * point.getY() + alpha * originalY; + double expectedZ = (1.0 - alpha) * point.getZ() + alpha * originalZ; + + assertEquals(expectedX, tuple.getX(), getEpsilon()); + assertEquals(expectedY, tuple.getY(), getEpsilon()); + assertEquals(expectedZ, tuple.getZ(), getEpsilon()); + } + } + + @Test + public void testSetOther() + { + Random random = new Random(621541L); + for (int i = 0; i < 1000; ++i) + { + alpha = random.nextDouble(0.0, 1.0); + AlphaFilteredTuple3D tuple = createRandomTuple(random); + + double originalX = tuple.getX(); + double originalY = tuple.getY(); + double originalZ = tuple.getZ(); + + Point3D point = new Point3D(createRandomTuple(random)); + tuple.set(point); + + double expectedX = (1.0 - alpha) * point.getX() + alpha * originalX; + double expectedY = (1.0 - alpha) * point.getY() + alpha * originalY; + double expectedZ = (1.0 - alpha) * point.getZ() + alpha * originalZ; + + assertEquals(expectedX, tuple.getX(), getEpsilon()); + assertEquals(expectedY, tuple.getY(), getEpsilon()); + assertEquals(expectedZ, tuple.getZ(), getEpsilon()); + } + } + + @Override + public AlphaFilteredTuple3D createEmptyTuple() + { + return new AlphaFilteredTuple3D(() -> alpha); + } + + @Override + public AlphaFilteredTuple3D createTuple(double v, double v1, double v2) + { + return new AlphaFilteredTuple3D(v, v1, v2, () -> alpha); + } + + @Override + public AlphaFilteredTuple3D createRandomTuple(Random random) + { + return new AlphaFilteredTuple3D(random.nextDouble(), random.nextDouble(), random.nextDouble(), () -> alpha); + } + + @Override + public double getEpsilon() + { + return Epsilons.ONE_TEN_MILLIONTH; + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2DTest.java new file mode 100644 index 00000000..eb861ee5 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2DTest.java @@ -0,0 +1,54 @@ +package us.ihmc.yoVariables.euclid.filters; + +import java.util.Random; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tuple2D.Point2D; +import us.ihmc.yoVariables.filters.AlphaFilteredYoVariable; +import us.ihmc.yoVariables.registry.YoRegistry; + +public class AlphaFilteredYoFramePoint2DTest +{ + private static final double EPSILON = 1.0e-15; + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + @Test + public void testConsistencyWithAlphaFilteredYoVariable() + { + Random random = new Random(3453456); + + for (int i = 0; i < 100; i++) + { + double alpha = random.nextDouble(); + YoRegistry registry = new YoRegistry("blop"); + + AlphaFilteredYoFramePoint2D filteredPoint = new AlphaFilteredYoFramePoint2D("tested", "", registry, alpha, ReferenceFrame.getWorldFrame()); + AlphaFilteredYoVariable xFiltered = new AlphaFilteredYoVariable("xRef", registry, alpha); + AlphaFilteredYoVariable yFiltered = new AlphaFilteredYoVariable("yRef", registry, alpha); + + Point2D unfilteredPoint = new Point2D(); + + for (int j = 0; j < 10; j++) + { + unfilteredPoint.add(EuclidCoreRandomTools.nextPoint2D(random, 0.0, 0.5)); + + filteredPoint.update(unfilteredPoint); + xFiltered.update(unfilteredPoint.getX()); + yFiltered.update(unfilteredPoint.getY()); + + EuclidCoreTestTools.assertEquals(new Point2D(xFiltered.getValue(), yFiltered.getValue()), filteredPoint, EPSILON); + } + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java new file mode 100644 index 00000000..abbe1fa5 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java @@ -0,0 +1,56 @@ +package us.ihmc.yoVariables.euclid.filters; + +import java.util.Random; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.yoVariables.filters.AlphaFilteredYoVariable; +import us.ihmc.yoVariables.registry.YoRegistry; + +public class AlphaFilteredYoFramePoint3DTest +{ + private static final double EPSILON = 1.0e-15; + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + @Test + public void testConsistencyWithAlphaFilteredYoVariable() + { + Random random = new Random(3453456); + + for (int i = 0; i < 100; i++) + { + double alpha = random.nextDouble(); + YoRegistry registry = new YoRegistry("blop"); + + AlphaFilteredYoFramePoint3D filteredPoint = new AlphaFilteredYoFramePoint3D("tested", "", registry, alpha, ReferenceFrame.getWorldFrame()); + AlphaFilteredYoVariable xFiltered = new AlphaFilteredYoVariable("xRef", registry, alpha); + AlphaFilteredYoVariable yFiltered = new AlphaFilteredYoVariable("yRef", registry, alpha); + AlphaFilteredYoVariable zFiltered = new AlphaFilteredYoVariable("zRef", registry, alpha); + + Point3D unfilteredPoint = new Point3D(); + + for (int j = 0; j < 10; j++) + { + unfilteredPoint.add(EuclidCoreRandomTools.nextPoint3D(random, 0.0, 0.5)); + + filteredPoint.update(unfilteredPoint); + xFiltered.update(unfilteredPoint.getX()); + yFiltered.update(unfilteredPoint.getY()); + zFiltered.update(unfilteredPoint.getZ()); + + EuclidCoreTestTools.assertEquals(new Point3D(xFiltered.getValue(), yFiltered.getValue(), zFiltered.getValue()), filteredPoint, EPSILON); + } + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java new file mode 100644 index 00000000..3d3998fa --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java @@ -0,0 +1,158 @@ +package us.ihmc.yoVariables.euclid.filters; + +import java.util.Random; + +import org.apache.commons.lang3.mutable.MutableDouble; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.axisAngle.AxisAngle; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; + +import static org.junit.jupiter.api.Assertions.*; + +public class AlphaFilteredYoFrameQuaternionTest +{ + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + @Test + public void testInitialValue() + { + MutableDouble alpha = new MutableDouble(); + AlphaFilteredYoFrameQuaternion q = createAlphaFilteredYoFrameQuaternion(() -> alpha.doubleValue()); + + // set measurement randomly + Random random = new Random(12351235L); + Quaternion qMeasured = EuclidCoreRandomTools.nextQuaternion(random); + q.getUnfilteredQuaternion().set(qMeasured); + + // call update once + q.update(); + Quaternion qFiltered = new Quaternion(q); + + // verify measurement equals filtered + EuclidCoreTestTools.assertEquals(qMeasured, qFiltered, 1e-12); + } + + @Test + public void testAlpha1() + { + MutableDouble alpha = new MutableDouble(); + AlphaFilteredYoFrameQuaternion q = createAlphaFilteredYoFrameQuaternion(() -> alpha.doubleValue()); + alpha.setValue(1.0); + + Random random = new Random(73464L); + + // update once + Quaternion qInitial = EuclidCoreRandomTools.nextQuaternion(random); + q.getUnfilteredQuaternion().set(qInitial); + q.update(); + + // update 100 more times + int nUpdates = 100; + doRandomUpdates(q, random, nUpdates); + + Quaternion qFiltered = new Quaternion(q); + + EuclidCoreTestTools.assertOrientation3DGeometricallyEquals(qInitial, qFiltered, 1e-12); + } + + @Test + public void testAlpha0() + { + MutableDouble alpha = new MutableDouble(); + AlphaFilteredYoFrameQuaternion q = createAlphaFilteredYoFrameQuaternion(() -> alpha.doubleValue()); + alpha.setValue(0.0); + + Random random = new Random(12525123L); + + // update 100 times + int nUpdates = 100; + doRandomUpdates(q, random, nUpdates); + + // update one more time + Quaternion qFinal = EuclidCoreRandomTools.nextQuaternion(random); + q.getUnfilteredQuaternion().set(qFinal); + q.update(); + + Quaternion qFiltered = new Quaternion(q); + + EuclidCoreTestTools.assertOrientation3DGeometricallyEquals(qFinal, qFiltered, 1e-12); + } + + @Test + public void testStepChange() + { + MutableDouble alpha = new MutableDouble(); + AlphaFilteredYoFrameQuaternion q = createAlphaFilteredYoFrameQuaternion(() -> alpha.doubleValue()); + alpha.setValue(0.5); + + Random random = new Random(12525123L); + + // update once + Quaternion qInitial = EuclidCoreRandomTools.nextQuaternion(random); + q.getUnfilteredQuaternion().set(qInitial); + q.update(); + + // update a whole bunch of times using the same quaternion + Quaternion qFinal = EuclidCoreRandomTools.nextQuaternion(random); + q.getUnfilteredQuaternion().set(qFinal); + + double angleDifference = getAngleDifference(qInitial, qFinal); + double epsilon = 1e-3; + + int nUpdates = 100; + Quaternion qFiltered = new Quaternion(); + for (int i = 0; i < nUpdates; i++) + { + q.update(); + qFiltered.set(q); + double newAngleDifference = getAngleDifference(qFiltered, qFinal); + // System.out.println(i + ": " + newAngleDifference); + boolean sameQuaternion = newAngleDifference == 0.0; + assertTrue(sameQuaternion || newAngleDifference < (1.0 + epsilon) * alpha.doubleValue() * angleDifference); + angleDifference = newAngleDifference; + } + } + + private void doRandomUpdates(AlphaFilteredYoFrameQuaternion q, Random random, int nUpdates) + { + for (int i = 0; i < nUpdates; i++) + { + // set measurement randomly and updated filtered version + Quaternion qMeasured = EuclidCoreRandomTools.nextQuaternion(random); + q.getUnfilteredQuaternion().set(qMeasured); + q.update(); + } + } + + private AlphaFilteredYoFrameQuaternion createAlphaFilteredYoFrameQuaternion(DoubleProvider alpha) + { + YoRegistry registry = new YoRegistry("test"); + ReferenceFrame referenceFrame = ReferenceFrame.getWorldFrame(); + YoFrameQuaternion unfilteredQuaternion = new YoFrameQuaternion("qMeasured", referenceFrame, registry); + AlphaFilteredYoFrameQuaternion q = new AlphaFilteredYoFrameQuaternion("qFiltered", "", unfilteredQuaternion, alpha, registry); + return q; + } + + private static double getAngleDifference(Quaternion q1, Quaternion q2) + { + Quaternion qDifference = new Quaternion(q1); + qDifference.multiplyConjugateOther(q2); + AxisAngle axisAngle = new AxisAngle(); + axisAngle.set(qDifference); + return axisAngle.getAngle(); + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2DTest.java new file mode 100644 index 00000000..38aefa84 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2DTest.java @@ -0,0 +1,54 @@ +package us.ihmc.yoVariables.euclid.filters; + +import java.util.Random; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tuple2D.Vector2D; +import us.ihmc.yoVariables.filters.AlphaFilteredYoVariable; +import us.ihmc.yoVariables.registry.YoRegistry; + +public class AlphaFilteredYoFrameVector2DTest +{ + private static final double EPSILON = 1.0e-15; + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + @Test + public void testConsistencyWithAlphaFilteredYoVariable() + { + Random random = new Random(3453456); + + for (int i = 0; i < 100; i++) + { + double alpha = random.nextDouble(); + YoRegistry registry = new YoRegistry("blop"); + + AlphaFilteredYoFrameVector2D filteredVector = new AlphaFilteredYoFrameVector2D("tested", "", registry, alpha, ReferenceFrame.getWorldFrame()); + AlphaFilteredYoVariable xFiltered = new AlphaFilteredYoVariable("xRef", registry, alpha); + AlphaFilteredYoVariable yFiltered = new AlphaFilteredYoVariable("yRef", registry, alpha); + + Vector2D unfilteredVector = new Vector2D(); + + for (int j = 0; j < 10; j++) + { + unfilteredVector.add(EuclidCoreRandomTools.nextVector2D(random, 0.0, 0.5)); + + filteredVector.update(unfilteredVector); + xFiltered.update(unfilteredVector.getX()); + yFiltered.update(unfilteredVector.getY()); + + EuclidCoreTestTools.assertEquals(new Vector2D(xFiltered.getValue(), yFiltered.getValue()), filteredVector, EPSILON); + } + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java new file mode 100644 index 00000000..3d4a6c3f --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java @@ -0,0 +1,57 @@ +package us.ihmc.yoVariables.euclid.filters; + +import java.util.Random; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.yoVariables.filters.AlphaFilteredYoVariable; +import us.ihmc.yoVariables.registry.YoRegistry; + +public class AlphaFilteredYoFrameVector3DTest +{ + private static final double EPSILON = 1.0e-15; + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + @Test + public void testConsistencyWithAlphaFilteredYoVariable() + { + Random random = new Random(3453456); + + for (int i = 0; i < 100; i++) + { + double alpha = random.nextDouble(); + YoRegistry registry = new YoRegistry("blop"); + + AlphaFilteredYoFrameVector3D filteredVector = new AlphaFilteredYoFrameVector3D("tested", "", registry, alpha, ReferenceFrame.getWorldFrame()); + AlphaFilteredYoVariable xFiltered = new AlphaFilteredYoVariable("xRef", registry, alpha); + AlphaFilteredYoVariable yFiltered = new AlphaFilteredYoVariable("yRef", registry, alpha); + AlphaFilteredYoVariable zFiltered = new AlphaFilteredYoVariable("zRef", registry, alpha); + + Vector3D unfilteredVector = new Vector3D(); + + for (int j = 0; j < 10; j++) + { + unfilteredVector.add(EuclidCoreRandomTools.nextPoint3D(random, 0.0, 0.5)); + + filteredVector.update(unfilteredVector); + xFiltered.update(unfilteredVector.getX()); + yFiltered.update(unfilteredVector.getY()); + zFiltered.update(unfilteredVector.getZ()); + + EuclidCoreTestTools.assertEquals(new Point3D(xFiltered.getValue(), yFiltered.getValue(), zFiltered.getValue()), filteredVector, EPSILON); + } + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/EuclidCoreTestMissingTools.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/EuclidCoreTestMissingTools.java new file mode 100644 index 00000000..6b8ff355 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/EuclidCoreTestMissingTools.java @@ -0,0 +1,34 @@ +package us.ihmc.yoVariables.euclid.filters; + +import us.ihmc.euclid.tools.EuclidCoreIOTools; +import us.ihmc.euclid.transform.RigidBodyTransform; + +public class EuclidCoreTestMissingTools +{ + public static String toStringFullPrecision(RigidBodyTransform rigidBodyTransform) + { + return EuclidCoreIOTools.getRigidBodyTransformString(EuclidCoreIOTools.getStringFormat(18, 18), rigidBodyTransform); + } + + public static RigidBodyTransform newRigidBodyTransformFromString(String rigidBodyTransformAsString) + { + String[] lines = rigidBodyTransformAsString.split("\\R"); // Split the input by newlines [3] + String[] tokens; + double[] doubles = new double[lines.length * 4]; // Initialize an array to store the doubles + int index = 0; + + for (String line : lines) + { + line = line.replace("|", "").trim(); // Remove the '|' character and trim whitespace + tokens = line.split("\\s+"); // Split the line by whitespace [2] + + for (String token : tokens) + { + doubles[index++] = Double.parseDouble(token); // Convert the token to a double and store it in the array [1] + } + } + + RigidBodyTransform rigidBodyTransform = new RigidBodyTransform(); + return new RigidBodyTransform(doubles); + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector2DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector2DTest.java new file mode 100644 index 00000000..058a6e13 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector2DTest.java @@ -0,0 +1,55 @@ +package us.ihmc.yoVariables.euclid.filters; + +import java.util.Random; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tuple2D.Point2D; +import us.ihmc.yoVariables.filters.FilteredFiniteDifferenceYoVariable; +import us.ihmc.yoVariables.registry.YoRegistry; + +public class FilteredVelocityYoFrameVector2DTest +{ + private static final double EPSILON = 1.0e-15; + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + @Test + public void testConsistencyWithAlphaFilteredYoVariable() + { + Random random = new Random(3453456); + + for (int i = 0; i < 100; i++) + { + double dt = EuclidCoreRandomTools.nextDouble(random, 1.0e-7, 1.0e-1); + double alpha = random.nextDouble(); + YoRegistry registry = new YoRegistry("blop"); + + FilteredFiniteDifferenceYoFrameVector2D filteredPoint = new FilteredFiniteDifferenceYoFrameVector2D("tested", "", () -> alpha, dt, registry, ReferenceFrame.getWorldFrame()); + FilteredFiniteDifferenceYoVariable xFiltered = new FilteredFiniteDifferenceYoVariable("xRef", "", alpha, dt, registry); + FilteredFiniteDifferenceYoVariable yFiltered = new FilteredFiniteDifferenceYoVariable("yRef", "", alpha, dt, registry); + + Point2D unfilteredPoint = new Point2D(); + + for (int j = 0; j < 10; j++) + { + unfilteredPoint.scaleAdd(dt, EuclidCoreRandomTools.nextPoint2D(random), unfilteredPoint); + + filteredPoint.update(unfilteredPoint); + xFiltered.update(unfilteredPoint.getX()); + yFiltered.update(unfilteredPoint.getY()); + + EuclidCoreTestTools.assertEquals(new Point2D(xFiltered.getValue(), yFiltered.getValue()), filteredPoint, EPSILON); + } + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector3DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector3DTest.java new file mode 100644 index 00000000..993193d2 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector3DTest.java @@ -0,0 +1,57 @@ +package us.ihmc.yoVariables.euclid.filters; + +import java.util.Random; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tuple3D.Point3D; +import us.ihmc.yoVariables.filters.FilteredFiniteDifferenceYoVariable; +import us.ihmc.yoVariables.registry.YoRegistry; + +public class FilteredVelocityYoFrameVector3DTest +{ + private static final double EPSILON = 1.0e-15; + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + @Test + public void testConsistencyWithAlphaFilteredYoVariable() + { + Random random = new Random(3453456); + + for (int i = 0; i < 100; i++) + { + double dt = EuclidCoreRandomTools.nextDouble(random, 1.0e-7, 1.0e-1); + double alpha = random.nextDouble(); + YoRegistry registry = new YoRegistry("blop"); + + FilteredFiniteDifferenceYoFrameVector3D filteredPoint = new FilteredFiniteDifferenceYoFrameVector3D("tested", "", () -> alpha, dt, registry, ReferenceFrame.getWorldFrame()); + FilteredFiniteDifferenceYoVariable xFiltered = new FilteredFiniteDifferenceYoVariable("xRef", "", alpha, dt, registry); + FilteredFiniteDifferenceYoVariable yFiltered = new FilteredFiniteDifferenceYoVariable("yRef", "", alpha, dt, registry); + FilteredFiniteDifferenceYoVariable zFiltered = new FilteredFiniteDifferenceYoVariable("zRef", "", alpha, dt, registry); + + Point3D unfilteredPoint = new Point3D(); + + for (int j = 0; j < 10; j++) + { + unfilteredPoint.scaleAdd(dt, EuclidCoreRandomTools.nextPoint3D(random), unfilteredPoint); + + filteredPoint.update(unfilteredPoint); + xFiltered.update(unfilteredPoint.getX()); + yFiltered.update(unfilteredPoint.getY()); + zFiltered.update(unfilteredPoint.getZ()); + + EuclidCoreTestTools.assertEquals(new Point3D(xFiltered.getValue(), yFiltered.getValue(), zFiltered.getValue()), filteredPoint, EPSILON); + } + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java new file mode 100644 index 00000000..f67bfce0 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java @@ -0,0 +1,81 @@ +package us.ihmc.yoVariables.euclid.filters; + +import java.util.Random; + +import org.apache.commons.lang3.mutable.MutableDouble; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.commons.RandomNumbers; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.yoVariables.filters.AngleTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; + +import static org.junit.jupiter.api.Assertions.*; + +public class RateLimitedYoFrameOrientationTest +{ + private static final double EPSILON = 2.0e-11; + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + @Test + public void testConvergenceWithConstantInput() throws Exception + { + Random random = new Random(46363); + double dt = 0.004; + MutableDouble maxRate = new MutableDouble(); + YoRegistry registry = new YoRegistry("dummy"); + RateLimitedYoFrameOrientation rateLimitedOrientation = new RateLimitedYoFrameOrientation("blop", "", registry, + (DoubleProvider) () -> maxRate.doubleValue(), dt, + ReferenceFrame.getWorldFrame()); + rateLimitedOrientation.update(new Quaternion()); + + FiniteDifferenceAngularVelocityYoFrameVector3D angularVelocity = new FiniteDifferenceAngularVelocityYoFrameVector3D("rate", ReferenceFrame.getWorldFrame(), + dt, registry); + + for (int i = 0; i < 1000; i++) + { + maxRate.setValue(RandomNumbers.nextDouble(random, 0.001, 10.0)); + Quaternion goalQuaternion = EuclidCoreRandomTools.nextQuaternion(random); + + double distanceToGoal = Math.abs(AngleTools.trimAngleMinusPiToPi(new Quaternion(rateLimitedOrientation).distance(goalQuaternion))); + if (distanceToGoal / dt < maxRate.doubleValue()) + { // Should converge in one step + rateLimitedOrientation.update(goalQuaternion); + EuclidCoreTestTools.assertOrientation3DGeometricallyEquals(goalQuaternion, new Quaternion(rateLimitedOrientation), EPSILON); + } + else + { + double timeToConverge = distanceToGoal / maxRate.doubleValue(); + int numberOfIterations = (int) (timeToConverge / dt); + double previousDistance = distanceToGoal; + angularVelocity.update(rateLimitedOrientation); + + for (int j = 0; j < numberOfIterations; j++) + { + rateLimitedOrientation.update(goalQuaternion); + double distance = Math.abs(AngleTools.trimAngleMinusPiToPi(new Quaternion(rateLimitedOrientation).distance(goalQuaternion))); + assertTrue(distance < previousDistance); + angularVelocity.update(rateLimitedOrientation); + double rate = angularVelocity.norm(); + assertEquals(rate, maxRate.doubleValue(), EPSILON, "difference: " + Math.abs(rate - maxRate.doubleValue())); + previousDistance = distance; + assertFalse(new Quaternion(rateLimitedOrientation).geometricallyEquals(goalQuaternion, EPSILON)); + } + + rateLimitedOrientation.update(goalQuaternion); + EuclidCoreTestTools.assertOrientation3DGeometricallyEquals(goalQuaternion, new Quaternion(rateLimitedOrientation), EPSILON); + } + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java new file mode 100644 index 00000000..f40e68d9 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java @@ -0,0 +1,79 @@ +package us.ihmc.yoVariables.euclid.filters; + +import static org.junit.jupiter.api.Assertions.*; + +import java.util.Random; + +import org.apache.commons.lang3.mutable.MutableDouble; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.commons.RandomNumbers; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.tuple4D.Quaternion; +import us.ihmc.yoVariables.filters.AngleTools; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoRegistry; + +public class RateLimitedYoFrameQuaternionTest +{ + private static final double EPSILON = 1.0e-12; + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + @Test + public void testConvergenceWithConstantInput() + { + Random random = new Random(46363); + double dt = 0.004; + MutableDouble maxRate = new MutableDouble(); + YoRegistry registry = new YoRegistry("dummy"); + RateLimitedYoFrameQuaternion rateLimitedQuaternion = new RateLimitedYoFrameQuaternion("blop", "", registry, (DoubleProvider) () -> maxRate.doubleValue(), + dt, ReferenceFrame.getWorldFrame()); + rateLimitedQuaternion.update(new Quaternion()); + + FiniteDifferenceAngularVelocityYoFrameVector3D angularVelocity = new FiniteDifferenceAngularVelocityYoFrameVector3D("rate", rateLimitedQuaternion, dt, registry); + + for (int i = 0; i < 1000; i++) + { + maxRate.setValue(RandomNumbers.nextDouble(random, 0.001, 10.0)); + Quaternion goalQuaternion = EuclidCoreRandomTools.nextQuaternion(random); + + double distanceToGoal = Math.abs(AngleTools.trimAngleMinusPiToPi(rateLimitedQuaternion.distance(goalQuaternion))); + if (distanceToGoal / dt < maxRate.doubleValue()) + { // Should converge in one step + rateLimitedQuaternion.update(goalQuaternion); + EuclidCoreTestTools.assertOrientation3DGeometricallyEquals(goalQuaternion, rateLimitedQuaternion, EPSILON); + } + else + { + double timeToConverge = distanceToGoal / maxRate.doubleValue(); + int numberOfIterations = (int) (timeToConverge / dt); + double previousDistance = distanceToGoal; + angularVelocity.update(); + + for (int j = 0; j < numberOfIterations; j++) + { + rateLimitedQuaternion.update(goalQuaternion); + double distance = Math.abs(AngleTools.trimAngleMinusPiToPi(rateLimitedQuaternion.distance(goalQuaternion))); + assertTrue(distance < previousDistance); + angularVelocity.update(); + double rate = angularVelocity.norm(); + assertEquals(rate, maxRate.doubleValue(), EPSILON, "difference: " + Math.abs(rate - maxRate.doubleValue())); + previousDistance = distance; + assertFalse(rateLimitedQuaternion.geometricallyEquals(goalQuaternion, EPSILON)); + } + + rateLimitedQuaternion.update(goalQuaternion); + EuclidCoreTestTools.assertOrientation3DGeometricallyEquals(goalQuaternion, rateLimitedQuaternion, EPSILON); + } + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java new file mode 100644 index 00000000..5ff399ef --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java @@ -0,0 +1,44 @@ +package us.ihmc.yoVariables.euclid.filters; + +import static org.junit.jupiter.api.Assertions.*; + +import java.util.Random; + +import org.apache.commons.math3.stat.descriptive.moment.Mean; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; + +public class RunningAverageYoFrameVector3DTest +{ + @Test + public void testAgainstApacheMean() + { + Random random = new Random(1); + RunningAverageYoFrameVector3D yoAverage = new RunningAverageYoFrameVector3D("", ReferenceFrame.getWorldFrame(), null); + Mean meanX = new Mean(); + Mean meanY = new Mean(); + Mean meanZ = new Mean(); + double nextX = -10.0; + double nextY = -10.0; + double nextZ = -10.0; + + for (int i = 0; i < 1000; i++) + { + nextX += random.nextDouble(); + nextY += random.nextDouble(); + nextZ += random.nextDouble(); + yoAverage.update(nextX, nextY, nextZ); + meanX.increment(nextX); + meanY.increment(nextY); + meanZ.increment(nextZ); + + assertEquals(meanX.getResult(), yoAverage.getX()); + assertEquals(meanY.getResult(), yoAverage.getY()); + assertEquals(meanZ.getResult(), yoAverage.getZ()); + assertEquals(meanX.getN(), yoAverage.getSampleSize()); + assertEquals(meanY.getN(), yoAverage.getSampleSize()); + assertEquals(meanZ.getN(), yoAverage.getSampleSize()); + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariableTest.java new file mode 100644 index 00000000..5ae32fb7 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariableTest.java @@ -0,0 +1,218 @@ +package us.ihmc.yoVariables.filters; + +import java.util.Random; + +import org.junit.jupiter.api.Test; + +import us.ihmc.commons.RandomNumbers; +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +import static org.junit.jupiter.api.Assertions.*; + +public class AlphaFilteredWrappingYoVariableTest +{ + private static final boolean DEBUG = false; + private final Random random = new Random(); + + @Test + public void testInputModulo() + { + YoRegistry registry = new YoRegistry("testRegistry"); + YoDouble alpha = new YoDouble("alpha", registry); + alpha.set(0.0); //sets the alpha to 0.0 so that the correction is instantaneous to check the result which is in fact the input with modulo + + YoDouble positionVariable = new YoDouble("positionVariable", registry); + AlphaFilteredWrappingYoVariable alphaFilteredWrappingYoVariable = new AlphaFilteredWrappingYoVariable("alphaFilteredWrappingYoVariable", "", registry, positionVariable, alpha, -2.0, 8.0); + + //test at the boundaries + positionVariable.set(8.0); + alphaFilteredWrappingYoVariable.update(); + assertTrue(MathTools.epsilonEquals(alphaFilteredWrappingYoVariable.getDoubleValue(), -2.0, 1e-10)); + + positionVariable.set(-2.0); + alphaFilteredWrappingYoVariable.update(); + assertTrue(MathTools.epsilonEquals(alphaFilteredWrappingYoVariable.getDoubleValue(), -2.0, 1e-10)); + + //test when the input is over the upperLimit + positionVariable.set(33.0); + alphaFilteredWrappingYoVariable.update(); + assertTrue(MathTools.epsilonEquals(alphaFilteredWrappingYoVariable.getDoubleValue(), 3.0, 1e-10)); + + positionVariable.set(38.0); + alphaFilteredWrappingYoVariable.update(); + assertTrue(MathTools.epsilonEquals(alphaFilteredWrappingYoVariable.getDoubleValue(), -2.0, 1e-10)); + + positionVariable.set(42.0); + alphaFilteredWrappingYoVariable.update(); + assertTrue(MathTools.epsilonEquals(alphaFilteredWrappingYoVariable.getDoubleValue(), 2.0, 1e-10)); + + //test when the input is under the lowerLimit + positionVariable.set(-22.0); + alphaFilteredWrappingYoVariable.update(); + assertTrue(MathTools.epsilonEquals(alphaFilteredWrappingYoVariable.getDoubleValue(), -2.0, 1e-10)); + + positionVariable.set(-23.5); + alphaFilteredWrappingYoVariable.update(); + assertTrue(MathTools.epsilonEquals(alphaFilteredWrappingYoVariable.getDoubleValue(), 6.5, 1e-10)); + + positionVariable.set(-42.0); + alphaFilteredWrappingYoVariable.update(); + assertTrue(MathTools.epsilonEquals(alphaFilteredWrappingYoVariable.getDoubleValue(), -2.0, 1e-10)); + } + + + @Test + public void testNoisyFixedPosition() + { + // Use a reasonably large alpha for a reasonably large amount of noise + YoRegistry registry = new YoRegistry("testRegistry"); + YoDouble alpha = new YoDouble("alpha", registry); + alpha.set(0.8); + + YoDouble positionVariable = new YoDouble("positionVariable", registry); + AlphaFilteredWrappingYoVariable alphaFilteredWrappingYoVariable = new AlphaFilteredWrappingYoVariable("alphaFilteredWrappingYoVariable", "", registry, positionVariable, alpha, 0.0, 20.0); + + double pseudoNoise = 0; + + positionVariable.set(10.0); + 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 = random.nextDouble(); + } + positionVariable.add(Math.pow(-1, i) * pseudoNoise); + alphaFilteredWrappingYoVariable.update(); + } + + assertEquals(10.0, alphaFilteredWrappingYoVariable.getDoubleValue(), 1.0); + } + + @Test + public void testErrorAlwaysDecreases() + { + // Use a reasonably large alpha for a reasonably large amount of noise + YoRegistry registry = new YoRegistry("testRegistry"); + YoDouble alpha = new YoDouble("alpha", registry); + alpha.set(0.999999); + + YoDouble positionVariable = new YoDouble("positionVariable", registry); + double lowerLimit = RandomNumbers.nextDouble(random, -100.0, 100.0); + double upperLimit = RandomNumbers.nextDouble(random, -100.0, 100.0); + if(upperLimit < lowerLimit) + { + double temp = lowerLimit; + lowerLimit = upperLimit; + upperLimit = temp; + } + + AlphaFilteredWrappingYoVariable alphaFilteredWrappingYoVariable = new AlphaFilteredWrappingYoVariable("alphaFilteredWrappingYoVariable", "", registry, positionVariable, alpha, lowerLimit, upperLimit); + positionVariable.set(RandomNumbers.nextDouble(random, lowerLimit, upperLimit)); + alphaFilteredWrappingYoVariable.update(); + + for(int iteration = 0; iteration < 10000; iteration++) + { + positionVariable.set(RandomNumbers.nextDouble(random, lowerLimit, upperLimit)); + double lastError = getErrorConsideringWrap(alphaFilteredWrappingYoVariable.getDoubleValue(), positionVariable.getDoubleValue(),lowerLimit, upperLimit); + for (int convergeAlphaCount = 0; convergeAlphaCount < 100; convergeAlphaCount++) + { + alphaFilteredWrappingYoVariable.update(); + double currentError = getErrorConsideringWrap(alphaFilteredWrappingYoVariable.getDoubleValue(), positionVariable.getDoubleValue(),lowerLimit, upperLimit); + assertTrue(Math.abs(currentError) < Math.abs(lastError)); + } + } + } + + public double getErrorConsideringWrap(double current, double target, double lowerLimit, double upperLimit) + { + double range = upperLimit - lowerLimit; + if(target > upperLimit) + { + target = (target - lowerLimit) % range + lowerLimit; + } + + if(target < lowerLimit) + { + double offset = (target - upperLimit) % range; + target = offset + upperLimit; + } + + double standardError = target - current; + double wrappingError = 0.0; + if(target > current) + { + wrappingError = lowerLimit - current + target - upperLimit; + } + else + { + wrappingError = upperLimit - current + target - lowerLimit; + } + + if(Math.abs(standardError) < Math.abs(wrappingError)) + { + return standardError; + } + return wrappingError; + } + + @Test + public void testWrappingError() + { + double e = getErrorConsideringWrap(0.2,0.8,0.0,1.0); + assertEquals(-0.4, e, 0.001); + + e = getErrorConsideringWrap(0.8,0.2,0.0,1.0); + assertEquals(0.4, e, 0.001); + + e = getErrorConsideringWrap(0.0,0.4,0,1); + assertEquals(0.4, e, 0.001); + + e = getErrorConsideringWrap(-0.2,0.4,-1.0,1.0); + assertEquals(0.6, e, 0.001); + + e = getErrorConsideringWrap(-1.0,1.0,-1.0,1.0); + assertEquals(0.0, e, 0.001); + + e = getErrorConsideringWrap(1.0,-1.0,-1.0,1.0); + assertEquals(0.0, e, 0.001); + + e = getErrorConsideringWrap(0.4, 1.6,-1.0,1.0); + assertEquals(-0.8, e, 0.001); + + e = getErrorConsideringWrap(-0.4, -1.6,-1.0,1.0); + assertEquals(0.8, e, 0.001); + + e = getErrorConsideringWrap(0.4, -1.6,-1.0,1.0); + assertEquals(0.0, e, 0.001); + + e = getErrorConsideringWrap(0.2, 0.2,-1.0,1.0); + assertEquals(0.0, e, 0.001); + + e = getErrorConsideringWrap(-3.2, -4.0,-5.0,-1.0); + assertEquals(-0.8, e, 0.001); + + e = getErrorConsideringWrap(0.0, 0.0, -1.0, 1.0); + + } + + @Test + public void testAlphaAndBreakFrequencyComputations() + { + double DT = 0.1; + double randomAlpha = random.nextDouble(); + double computedBreakFrequency = AlphaFilteredWrappingYoVariable.computeBreakFrequencyGivenAlpha(randomAlpha, DT); + double computedAlpha = AlphaFilteredWrappingYoVariable.computeAlphaGivenBreakFrequencyProperly(computedBreakFrequency, DT); + + assertEquals(randomAlpha, computedAlpha, 1e-7); + assertEquals(computedBreakFrequency, AlphaFilteredWrappingYoVariable.computeBreakFrequencyGivenAlpha(computedAlpha, DT), 1e-7); + + if(DEBUG) + { + System.out.println("Random Alpha: " + randomAlpha); + System.out.println("Computed Alpha: " + AlphaFilteredWrappingYoVariable.computeAlphaGivenBreakFrequencyProperly(computedBreakFrequency, DT)); + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariableTest.java new file mode 100644 index 00000000..43839c72 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariableTest.java @@ -0,0 +1,64 @@ +package us.ihmc.yoVariables.filters; + +import java.util.Random; + +import org.junit.jupiter.api.Test; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +import static org.junit.jupiter.api.Assertions.*; + +public class AlphaFilteredYoVariableTest +{ + private final Random rng = new Random(); + + @Test + public void testNoisyFixedPosition() + { + // Use a reasonably large alpha for a reasonably large amount of noise + double alpha = 0.8; + + YoRegistry registry = new YoRegistry("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); + } + + @Test + 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); + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariableTest.java new file mode 100644 index 00000000..83f543e5 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariableTest.java @@ -0,0 +1,373 @@ +package us.ihmc.yoVariables.filters; + +import org.junit.jupiter.api.Test; +import us.ihmc.commons.RandomNumbers; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +import java.util.Random; + +import static org.junit.jupiter.api.Assertions.*; + +public class BacklashCompensatingVelocityYoVariableTest +{ + private static final double EPSILON = 1e-8; + + @Test + public void testWithoutBacklashOrFiltering1() + { + Random rand = new Random(1798L); + + YoRegistry registry = new YoRegistry("blop"); + YoDouble alphaVariable = new YoDouble("alpha", registry); + double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); + YoDouble slopTime = new YoDouble("slop", registry); + BacklashCompensatingVelocityYoVariable unprocessed = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, dt, slopTime, registry); + + double rawPosition = 0.0, rawPositionPrevValue = 0.0; + unprocessed.update(rawPosition); + + for (int i = 0; i < 1000; i++) + { + rawPosition = RandomNumbers.nextDouble(rand, -100.0, 100.0); + unprocessed.update(rawPosition); + + double rawVelocity = (rawPosition - rawPositionPrevValue) / dt; + + assertEquals(rawVelocity, unprocessed.getDoubleValue(), EPSILON); + + rawPositionPrevValue = rawPosition; + } + } + + @Test + public void testWithoutBacklashOrFiltering2() + { + Random rand = new Random(1798L); + + YoRegistry registry = new YoRegistry("blop"); + YoDouble alphaVariable = new YoDouble("alpha", registry); + double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); + YoDouble slopTime = new YoDouble("slop", registry); + YoDouble rawPosition = new YoDouble("rawPosition", registry); + BacklashCompensatingVelocityYoVariable unprocessed = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, rawPosition, dt, slopTime, + registry); + + double rawPositionPrevValue = 0.0; + unprocessed.update(); + + for (int i = 0; i < 1000; i++) + { + rawPosition.set(RandomNumbers.nextDouble(rand, -100.0, 100.0)); + unprocessed.update(); + + double rawVelocity = (rawPosition.getDoubleValue() - rawPositionPrevValue) / dt; + + assertEquals(rawVelocity, unprocessed.getDoubleValue(), EPSILON); + + rawPositionPrevValue = rawPosition.getDoubleValue(); + } + } + + @Test + public void testWithoutBacklash1() + { + Random rand = new Random(1798L); + + YoRegistry registry = new YoRegistry("blop"); + YoDouble alphaVariable = new YoDouble("alpha", registry); + alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); + double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); + YoDouble slopTime = new YoDouble("slop", registry); + YoDouble rawPosition = new YoDouble("rawPosition", registry); + FilteredFiniteDifferenceYoVariable filtVelocity = new FilteredFiniteDifferenceYoVariable("filtVelocity", "", alphaVariable, rawPosition, dt, registry); + BacklashCompensatingVelocityYoVariable filteredOnly = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, dt, slopTime, registry); + + filtVelocity.update(); + filteredOnly.update(rawPosition.getDoubleValue()); + + for (int i = 0; i < 1000; i++) + { + alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); + rawPosition.set(RandomNumbers.nextDouble(rand, -100.0, 100.0)); + filtVelocity.update(); + filteredOnly.update(rawPosition.getDoubleValue()); + + assertEquals(filtVelocity.getDoubleValue(), filteredOnly.getDoubleValue(), EPSILON); + } + } + + @Test + public void testWithoutBacklash2() + { + Random rand = new Random(1798L); + + YoRegistry registry = new YoRegistry("blop"); + YoDouble alphaVariable = new YoDouble("alpha", registry); + alphaVariable.set(RandomNumbers.nextDouble(rand, 0.0, 1.0)); + double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); + YoDouble slopTime = new YoDouble("slop", registry); + YoDouble rawPosition = new YoDouble("rawPosition", registry); + FilteredFiniteDifferenceYoVariable filtVelocity = new FilteredFiniteDifferenceYoVariable("filtVelocity", "", alphaVariable, rawPosition, dt, registry); + BacklashCompensatingVelocityYoVariable filteredOnly = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, rawPosition, dt, slopTime, + registry); + + filtVelocity.update(); + filteredOnly.update(); + + for (int i = 0; i < 1000; i++) + { + alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); + rawPosition.set(RandomNumbers.nextDouble(rand, -100.0, 100.0)); + filtVelocity.update(); + filteredOnly.update(); + + assertEquals(filtVelocity.getDoubleValue(), filteredOnly.getDoubleValue(), EPSILON); + } + } + + @Test + public void testVelocityPositiveWithoutCrossingZero2() + { + Random rand = new Random(1798L); + + YoRegistry registry = new YoRegistry("blop"); + YoDouble alphaVariable = new YoDouble("alpha", registry); + alphaVariable.set(RandomNumbers.nextDouble(rand, 0.0, 1.0)); + double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); + YoDouble slopTime = new YoDouble("slop", registry); + YoDouble rawPosition = new YoDouble("rawPosition", registry); + FilteredFiniteDifferenceYoVariable filtVelocity = new FilteredFiniteDifferenceYoVariable("filtVelocity", "", alphaVariable, rawPosition, dt, registry); + BacklashCompensatingVelocityYoVariable backlashAndFiltered = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, rawPosition, dt, slopTime, + registry); + + filtVelocity.update(); + backlashAndFiltered.update(); + + // In this test, the position is only increasing, so there should be no backlash filtering that gets applied. + + double currentTime = 0.0; + for (int i = 0; i < 10000; i++) + { + slopTime.set(RandomNumbers.nextDouble(rand, 0.0, 10.0)); + alphaVariable.set(RandomNumbers.nextDouble(rand, 0.1, 1.0)); + rawPosition.add(RandomNumbers.nextDouble(rand, 0.0, 101.0)); + filtVelocity.update(); + backlashAndFiltered.update(); + +// if (currentTime > 2.0 * slopTime.getDoubleValue()) + assertEquals(filtVelocity.getDoubleValue(), backlashAndFiltered.getDoubleValue(), EPSILON); + + currentTime += dt; + } + } + + @Test + public void testVelocityNegativeWithoutCrossingZero2() + { + Random rand = new Random(1798L); + + YoRegistry registry = new YoRegistry("blop"); + YoDouble alphaVariable = new YoDouble("alpha", registry); + alphaVariable.set(RandomNumbers.nextDouble(rand, 0.0, 1.0)); + double dt = RandomNumbers.nextDouble(rand, 1e-8, 1.0); + YoDouble slopTime = new YoDouble("slop", registry); + YoDouble rawPosition = new YoDouble("rawPosition", registry); + FilteredFiniteDifferenceYoVariable filtVelocity = new FilteredFiniteDifferenceYoVariable("filtVelocity", "", alphaVariable, rawPosition, dt, registry); + BacklashCompensatingVelocityYoVariable backlashAndFiltered = new BacklashCompensatingVelocityYoVariable("", "", alphaVariable, rawPosition, dt, slopTime, + registry); + + filtVelocity.update(); + backlashAndFiltered.update(); + + for (int i = 0; i < 1000; i++) + { + slopTime.set(RandomNumbers.nextDouble(rand, 0.0, 100.0)); + alphaVariable.set(RandomNumbers.nextDouble(rand, 0.0, 1.0)); + rawPosition.sub(RandomNumbers.nextDouble(rand, 0.0, 101.0)); + filtVelocity.update(); + backlashAndFiltered.update(); + + assertEquals(filtVelocity.getDoubleValue(), backlashAndFiltered.getDoubleValue(), EPSILON); + } + } + + + + @Test + public void testNoisySignalAndMakeSureVelocityHasSignalContent() + { + Random random = new Random(1798L); + + YoRegistry registry = new YoRegistry("Registry"); + YoDouble alphaVariable = new YoDouble("alpha", registry); + YoDouble slopTime = new YoDouble("slopTime", registry); + YoDouble cleanPosition = new YoDouble("cleanPosition", registry); + YoDouble noisyPosition = new YoDouble("noisyPosition", registry); + YoDouble cleanVelocity = new YoDouble("cleanVelocity", registry); + + + YoDouble reconstructedPosition = new YoDouble("reconstructedPosition", registry); + YoDouble reconstructedPosition2 = new YoDouble("reconstructedPosition2", registry); + + YoDouble totalReconstructedPositionError2 = new YoDouble("totalReconstructedPositionError2", registry); + + YoDouble averageReconstructedPositionError2 = new YoDouble("averageReconstructedPositionError2", registry); + + double dt = 0.001; + double totalTime = 5.0; + + double amplitude = 2.0; + double frequency = 1.0; + double noiseAmplitude = 0.01; + + slopTime.set(0.1); + alphaVariable.set(0.95); + + BacklashCompensatingVelocityYoVariable revisedBacklashCompensatingVelocity = new BacklashCompensatingVelocityYoVariable("bl_qd_velocity2", "", alphaVariable, noisyPosition, dt, slopTime, registry); + + reconstructedPosition2.set(amplitude); + +// SimulationConstructionSet scs = new SimulationConstructionSet(new Robot("Test")); +// scs.addYoVariableRegistry(registry); +// scs.startOnAThread(); + + for (double time = 0.0; time < totalTime; time = time + dt) + { + cleanPosition.set(amplitude * Math.cos(2.0 * Math.PI * frequency * time)); + cleanVelocity.set(-2.0 * Math.PI * amplitude * frequency * Math.sin(2.0 * Math.PI * frequency * time)); + + noisyPosition.set(cleanPosition.getDoubleValue()); + noisyPosition.add(RandomNumbers.nextDouble(random, noiseAmplitude)); + + revisedBacklashCompensatingVelocity.update(); + + reconstructedPosition2.add(revisedBacklashCompensatingVelocity.getDoubleValue() * dt); + + double positionError2 = reconstructedPosition2.getDoubleValue() - cleanPosition.getDoubleValue(); + totalReconstructedPositionError2.add(Math.abs(positionError2) * dt); + +// scs.tickAndUpdate(); + } + + averageReconstructedPositionError2.set(totalReconstructedPositionError2.getDoubleValue() / totalTime); + + // The original one doesn't do very well with noisy signals because it thinks the noise is backlash. + assertTrue(averageReconstructedPositionError2.getDoubleValue() < 0.25); + } + + @Test + public void testSignalWithBacklash() + { + YoRegistry registry = new YoRegistry("Registry"); + YoDouble alphaVariable = new YoDouble("alpha", registry); + YoDouble slopTime = new YoDouble("slopTime", registry); + + YoDouble cleanPosition = new YoDouble("cleanPosition", registry); + YoDouble backlashyPosition = new YoDouble("backlashyPosition", registry); + YoDouble cleanVelocity = new YoDouble("cleanVelocity", registry); + + + YoDouble reconstructedPosition2 = new YoDouble("reconstructedPosition2", registry); + + YoDouble totalReconstructedPositionError2 = new YoDouble("totalReconstructedPositionError2", registry); + + YoDouble averageReconstructedPositionError2 = new YoDouble("averageReconstructedPositionError2", registry); + + double dt = 0.001; + double totalTime = 5.0; + + double amplitude = 2.0; + double frequency = 1.0; + double backlashAmount = 0.1; + + slopTime.set(0.1); + alphaVariable.set(0.95); + + BacklashCompensatingVelocityYoVariable revisedBacklashCompensatingVelocity = new BacklashCompensatingVelocityYoVariable("bl_qd_velocity2", "", alphaVariable, backlashyPosition, dt, slopTime, registry); + + reconstructedPosition2.set(amplitude); + +// SimulationConstructionSet scs = new SimulationConstructionSet(new Robot("Test")); +// scs.addYoVariableRegistry(registry); +// scs.startOnAThread(); + + for (double time = 0.0; time < totalTime; time = time + dt) + { + cleanPosition.set(amplitude * Math.cos(2.0 * Math.PI * frequency * time)); + cleanVelocity.set(-2.0 * Math.PI * amplitude * frequency * Math.sin(2.0 * Math.PI * frequency * time)); + + backlashyPosition.set(cleanPosition.getDoubleValue()); + if(cleanVelocity.getDoubleValue() > 0.0) + { + backlashyPosition.add(backlashAmount); + } + + revisedBacklashCompensatingVelocity.update(); + + reconstructedPosition2.add(revisedBacklashCompensatingVelocity.getDoubleValue() * dt); + + double positionError2 = reconstructedPosition2.getDoubleValue() - cleanPosition.getDoubleValue(); + totalReconstructedPositionError2.add(Math.abs(positionError2) * dt); + +// scs.tickAndUpdate(); + } + + averageReconstructedPositionError2.set(totalReconstructedPositionError2.getDoubleValue() / totalTime); + + assertTrue(averageReconstructedPositionError2.getDoubleValue() < 0.25); + } + + + @Test + public void testRemoveSquareWaveBacklash() + { + YoRegistry registry = new YoRegistry("Registry"); + YoDouble alphaVariable = new YoDouble("alpha", registry); + YoDouble slopTime = new YoDouble("slopTime", registry); + + YoDouble backlashyPosition = new YoDouble("backlashyPosition", registry); + + double dt = 0.001; + double totalTime = 5.0; + + double frequency = 30.0; + double backlashAmount = 0.1; + + slopTime.set(0.1); + alphaVariable.set(0.95); + + BacklashCompensatingVelocityYoVariable revisedBacklashCompensatingVelocity = new BacklashCompensatingVelocityYoVariable("bl_qd_velocity2", "", alphaVariable, backlashyPosition, dt, slopTime, registry); + +// SimulationConstructionSet scs = new SimulationConstructionSet(new Robot("Test")); +// scs.addYoVariableRegistry(registry); +// scs.startOnAThread(); + + // initialize the system to make sure it's resting up against one of the heads of slop. Previously without this, it was a lucky test. + backlashyPosition.set(0.0); + revisedBacklashCompensatingVelocity.update(); + backlashyPosition.set(backlashAmount); + for (int i = 0; i < 2.0 * slopTime.getDoubleValue() / dt; i++) + revisedBacklashCompensatingVelocity.update(); + + + for (double time = 0.0; time < totalTime; time = time + dt) + { + backlashyPosition.set(Math.cos(2.0 * Math.PI * frequency * time)); + if (backlashyPosition.getDoubleValue() > 0.0) + { + backlashyPosition.set(backlashAmount); + } + else + { + backlashyPosition.set(-backlashAmount); + } + + revisedBacklashCompensatingVelocity.update(); + + assertEquals(0.0, revisedBacklashCompensatingVelocity.getDoubleValue(), 1e-3); + +// scs.tickAndUpdate(); + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariableTest.java new file mode 100644 index 00000000..ad9eefa9 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariableTest.java @@ -0,0 +1,46 @@ +package us.ihmc.yoVariables.filters; + + +import java.util.Random; + +import org.junit.jupiter.api.Test; + +import us.ihmc.commons.RandomNumbers; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +import static org.junit.jupiter.api.Assertions.*; + +public class BacklashProcessingYoVariableTest +{ + @Test + public void testAgainstRevisedBacklash() + { + YoRegistry registry = new YoRegistry("dummy"); + YoDouble slopTime = new YoDouble("slopTime", registry); + double dt = 0.002; + YoDouble alpha = new YoDouble("alpha", registry); + alpha.set(AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(16.0, dt)); + YoDouble positionVariable = new YoDouble("rawPosition", registry); + FilteredFiniteDifferenceYoVariable velocityVariable = new FilteredFiniteDifferenceYoVariable("fd", "", alpha, positionVariable, dt, registry); + + BacklashProcessingYoVariable blToTest = new BacklashProcessingYoVariable("blTest", "", velocityVariable, dt, slopTime, registry); + + BacklashCompensatingVelocityYoVariable blExpected = new BacklashCompensatingVelocityYoVariable("blExpected", "", alpha, positionVariable, dt, slopTime, registry); + + Random random = new Random(561651L); + + for (double t = 0.0; t < 100.0; t += dt) + { + positionVariable.set(2.0 * Math.sin(2.0 * Math.PI * 10.0) + RandomNumbers.nextDouble(random, 1.0) * Math.sin(2.0 * Math.PI * 30.0 + 2.0 / 3.0 * Math.PI)); + + velocityVariable.update(); + + blToTest.update(); + blExpected.update(); + + assertEquals(blToTest.getDoubleValue(), blExpected.getDoubleValue(), 1.0e-10); + } + } + +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeadbandedYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeadbandedYoVariableTest.java new file mode 100644 index 00000000..9b2af143 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeadbandedYoVariableTest.java @@ -0,0 +1,59 @@ +package us.ihmc.yoVariables.filters; + +import org.junit.jupiter.api.Test; + +import us.ihmc.commons.MathTools; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +import static org.junit.jupiter.api.Assertions.*; + +public class DeadbandedYoVariableTest +{ + + @Test + public void testDeadband() + { + YoRegistry registry = new YoRegistry("test"); + YoDouble deadzoneSize = new YoDouble("deadzoneSize", registry); + YoDouble input = new YoDouble("input", registry); + double deadzone = 2.0; + deadzoneSize.set(deadzone); + DeadbandedYoVariable testDeadzone = new DeadbandedYoVariable("testDeadZone", input , deadzoneSize, registry); + + double verySmallStep = 1e-4; + + input.set(deadzone - verySmallStep); + testDeadzone.update(); + assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), 0.0, 1e-14)); + input.set(deadzone + verySmallStep); + testDeadzone.update(); + assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), verySmallStep, 1e-14)); + + input.set(-deadzone + verySmallStep); + testDeadzone.update(); + assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), 0.0, 1e-14)); + input.set(-deadzone - verySmallStep); + testDeadzone.update(); + assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), -verySmallStep, 1e-14)); + + for (double valueToBeCorrected = -10.0; valueToBeCorrected < -deadzone; valueToBeCorrected += 0.01) + { + input.set(valueToBeCorrected); + testDeadzone.update(); + assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), valueToBeCorrected + deadzone, 1e-14)); + } + for (double valueToBeCorrected = -deadzone; valueToBeCorrected < deadzone; valueToBeCorrected += 0.01) + { + input.set(valueToBeCorrected); + testDeadzone.update(); + assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), 0.0, 1e-14)); + } + for (double valueToBeCorrected = deadzone; valueToBeCorrected < 10.0; valueToBeCorrected += 0.01) + { + input.set(valueToBeCorrected); + testDeadzone.update(); + assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), valueToBeCorrected - deadzone, 1e-14)); + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoBooleanTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoBooleanTest.java new file mode 100644 index 00000000..e99d33b6 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoBooleanTest.java @@ -0,0 +1,190 @@ +package us.ihmc.yoVariables.filters; + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +import static org.junit.jupiter.api.Assertions.*; + +public class DelayedYoBooleanTest +{ + private static YoRegistry registry; + private static YoBoolean variableToDelay; + private static Boolean DEBUG = false; + private int ticksToDelay; + + @BeforeEach + public void setUp() + { + registry = new YoRegistry("registry"); + variableToDelay = new YoBoolean("variableToDelay", registry); + } + + @AfterEach + public void tearDown() + { + registry = null; + variableToDelay = null; + } + + @Test + public void testDelayedYoVariableMultipleTickDelays() + { + for (ticksToDelay = 0; ticksToDelay < 10; ticksToDelay++) + { + variableToDelay.set(true); + + DelayedYoBoolean delayedYoVariable = new DelayedYoBoolean("delayedVariable" + ticksToDelay, "", variableToDelay, ticksToDelay, + registry); + + int ticksToTest = 100; + boolean[] valuesToSet = new boolean[ticksToTest]; + + for (int i = 0; i < valuesToSet.length; i++) + { + if (Math.random() < .5) + valuesToSet[i] = true; + else + valuesToSet[i] = false; + } + + assertEquals(delayedYoVariable.getBooleanValue(), true); + + for (int i = 0; i < ticksToTest; i++) + { + variableToDelay.set(valuesToSet[i]); + delayedYoVariable.update(); + + if (i < ticksToDelay) + { + assertEquals(delayedYoVariable.getBooleanValue(), true); + } + else + { + assertEquals(delayedYoVariable.getBooleanValue(), valuesToSet[i - ticksToDelay]); + } + } + } + } + + @Test + public void testDelayedYoVariableOneTickDelay() + { + ticksToDelay = 1; + + variableToDelay.set(false); + DelayedYoBoolean delayedYoVariable = new DelayedYoBoolean("delayedVariable" + ticksToDelay, "", variableToDelay, ticksToDelay, registry); + assertEquals(delayedYoVariable.getBooleanValue(), false); + + variableToDelay.set(true); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), false); + + variableToDelay.set(false); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), true); + + variableToDelay.set(true); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), false); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), true); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), true); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), true); + } + + @Test + public void testDelayedYoVariableZeroTickDelay() + { + ticksToDelay = 0; + + variableToDelay.set(false); + DelayedYoBoolean delayedYoVariable = new DelayedYoBoolean("delayedVariable" + ticksToDelay, "", variableToDelay, ticksToDelay, registry); + assertEquals(delayedYoVariable.getBooleanValue(), false); + + variableToDelay.set(true); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), true); + + variableToDelay.set(false); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), false); + + variableToDelay.set(true); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), true); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), true); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), true); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), true); + } + + @Test + public void testUpdateWithZero() + { + ticksToDelay = 0; + variableToDelay.set(false); + DelayedYoBoolean delayedYoVariable = new DelayedYoBoolean("delayedVariable" + ticksToDelay, "", variableToDelay, ticksToDelay, registry); + delayedYoVariable.getInternalState("Should be all false", DEBUG); + + variableToDelay.set(true); + delayedYoVariable.update(); + delayedYoVariable.getInternalState("Should be all true", DEBUG); + + assertEquals(delayedYoVariable.getBooleanValue(), true); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getBooleanValue(), true); + } + + @Test + public void testReset() + { + ticksToDelay = 10; + variableToDelay.set(false); + DelayedYoBoolean delayedYoVariable = new DelayedYoBoolean("delayedVariable" + ticksToDelay, "", variableToDelay, ticksToDelay, registry); + + for(int i = 0; i < ticksToDelay; i++) + { + assertEquals(delayedYoVariable.getBooleanValue(), false); + delayedYoVariable.update(); + } + + delayedYoVariable.getInternalState("Should be all false", DEBUG); + + variableToDelay.set(true); + delayedYoVariable.update(); + + for(int i = 0; i < ticksToDelay; i++) + { + assertEquals(delayedYoVariable.getBooleanValue(), false); + delayedYoVariable.update(); + } + assertEquals(delayedYoVariable.getBooleanValue(), true); + + variableToDelay.set(false); + delayedYoVariable.update(); + delayedYoVariable.getInternalState("Should be all true, except for the end", DEBUG); + + delayedYoVariable.reset(); + delayedYoVariable.update(); + + for(int i = 0; i < ticksToDelay; i++) + { + assertEquals(delayedYoVariable.getBooleanValue(), false); + delayedYoVariable.update(); + } + delayedYoVariable.getInternalState("Should be all false", DEBUG); + } +} \ No newline at end of file diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoDoubleTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoDoubleTest.java new file mode 100644 index 00000000..e2c46f34 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoDoubleTest.java @@ -0,0 +1,121 @@ +package us.ihmc.yoVariables.filters; + +import org.junit.jupiter.api.Test; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +import static org.junit.jupiter.api.Assertions.*; + +public class DelayedYoDoubleTest +{ + + @Test + public void testDelayedYoVariableMultipleTickDelays() + { + YoRegistry registry = new YoRegistry("registry"); + YoDouble variableToDelay = new YoDouble("variableToDelay", registry); + + for (int ticksToDelay = 0; ticksToDelay < 10; ticksToDelay++) + { + double firstValue = Math.random(); + variableToDelay.set(firstValue); + + DelayedYoDouble delayedYoVariable = new DelayedYoDouble("delayedVariable" + ticksToDelay, "", variableToDelay, ticksToDelay, registry); + + int ticksToTest = 100; + double[] valuesToSet = new double[ticksToTest]; + + for (int i = 0; i < valuesToSet.length; i++) + { + valuesToSet[i] = Math.random(); + } + + assertEquals(delayedYoVariable.getDoubleValue(), firstValue, 1e-7); + + for (int i = 0; i < ticksToTest; i++) + { + variableToDelay.set(valuesToSet[i]); + delayedYoVariable.update(); + + if (i < ticksToDelay) + { + assertEquals(delayedYoVariable.getDoubleValue(), firstValue, 1e-7); + } + else + { + assertEquals(delayedYoVariable.getDoubleValue(), valuesToSet[i - ticksToDelay], 1e-7); + } + } + } + + } + + @Test + public void testDelayedYoVariableOneTickDelay() + { + YoRegistry registry = new YoRegistry("registry"); + YoDouble variableToDelay = new YoDouble("variableToDelay", registry); + + int ticksToDelay = 1; + + variableToDelay.set(0.0); + DelayedYoDouble delayedYoVariable = new DelayedYoDouble("delayedVariable" + ticksToDelay, "", variableToDelay, ticksToDelay, registry); + assertEquals(delayedYoVariable.getDoubleValue(), 0.0, 1e-7); + + variableToDelay.set(1.0); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 0.0, 1e-7); + + variableToDelay.set(2.0); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 1.0, 1e-7); + + variableToDelay.set(3.0); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 2.0, 1e-7); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); + } + + @Test + public void testDelayedYoVariableZeroTickDelay() + { + YoRegistry registry = new YoRegistry("registry"); + YoDouble variableToDelay = new YoDouble("variableToDelay", registry); + + int ticksToDelay = 0; + + variableToDelay.set(0.0); + DelayedYoDouble delayedYoVariable = new DelayedYoDouble("delayedVariable" + ticksToDelay, "", variableToDelay, ticksToDelay, registry); + assertEquals(delayedYoVariable.getDoubleValue(), 0.0, 1e-7); + + variableToDelay.set(1.0); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 1.0, 1e-7); + + variableToDelay.set(2.0); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 2.0, 1e-7); + + variableToDelay.set(3.0); + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); + + delayedYoVariable.update(); + assertEquals(delayedYoVariable.getDoubleValue(), 3.0, 1e-7); + } + +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariableTest.java new file mode 100644 index 00000000..31d7b044 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariableTest.java @@ -0,0 +1,400 @@ +package us.ihmc.yoVariables.filters; + +import org.junit.jupiter.api.Test; +import us.ihmc.commons.RandomNumbers; +import us.ihmc.yoVariables.registry.YoRegistry; + +import java.util.Random; + +import static org.junit.jupiter.api.Assertions.*; + +/** + * @author Doug Stephen (dstephen@ihmc.us) + */ +public class DeltaLimitedYoVariableTest +{ + private static final int RANDOM_LOWER_BOUND = 10; + private static final int RANDOM_UPPER_BOUND = 30000; + private YoRegistry registry; + private DeltaLimitedYoVariable variable; + + @Test + public void testReferenceAndInputBothNegativeNoOvershootInputGreaterThanReference() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; + double input = reference / 2.0; + double delta = Math.abs(input - reference); + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + assertTrue(input < 0.0); + assertTrue(reference < 0.0); + assertTrue(input > reference); + assertFalse(variable.isLimitingActive()); + assertEquals(input, variable.getDoubleValue(), 1e-8); + } + } + + @Test + public void testReferenceAndInputBothNegativeNoOvershootReferenceGreaterThanInput() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; + double input = reference * 2; + double delta = Math.abs(input - reference); + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + assertTrue(input < 0.0); + assertTrue(reference < 0.0); + assertTrue(input < reference); + assertFalse(variable.isLimitingActive()); + assertEquals(input, variable.getDoubleValue(), 1e-8); + } + } + + @Test + public void testReferenceAndInputBothPositiveNoOvershootReferenceGreaterThanInput() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND); + double input = reference / 2.0; + double delta = Math.abs(input - reference); + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + assertTrue(input > 0.0); + assertTrue(reference > 0.0); + assertTrue(input < reference); + assertFalse(variable.isLimitingActive()); + assertEquals(input, variable.getDoubleValue(), 1e-8); + } + } + + @Test + public void testReferenceAndInputBothPositiveNoOvershootInputGreaterThanReference() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND); + double input = reference * 2; + double delta = Math.abs(input - reference); + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + assertTrue(input > 0.0); + assertTrue(reference > 0.0); + assertTrue(input > reference); + assertFalse(variable.isLimitingActive()); + assertEquals(input, variable.getDoubleValue(), 1e-8); + } + } + + @Test + public void testPositiveReferenceNegativeInputNoOvershoot() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND); + double input = reference * -0.5; + double delta = Math.abs(input - reference); + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + assertTrue(input < 0.0); + assertTrue(reference > 0.0); + assertTrue(input < reference); + assertFalse(variable.isLimitingActive()); + assertEquals(input, variable.getDoubleValue(), 1e-8); + } + } + + @Test + public void testNegativeReferencePositiveInputNoOvershoot() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; + double input = reference * -0.5; + double delta = Math.abs(input - reference); + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + assertTrue(input > 0.0); + assertTrue(reference < 0.0); + assertTrue(input > reference); + assertFalse(variable.isLimitingActive()); + assertEquals(input, variable.getDoubleValue(), 1e-8); + } + } + + @Test + public void testReferenceAndInputBothNegativeWithOvershootInputGreaterThanReference() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; + double input = reference / 2.0; + double delta = Math.abs(input - reference) / 2.0; + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + double overshoot = Math.abs(input - reference) - delta; + + double expectedClip = input - overshoot; + + assertTrue(input < 0.0); + assertTrue(reference < 0.0); + assertTrue(input > reference); + assertTrue(variable.isLimitingActive()); + assertEquals(expectedClip, + variable.getDoubleValue(), + 1e-8, + "Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs( + input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot); + } + } + + @Test + public void testReferenceAndInputBothNegativeWithOvershootReferenceGreaterThanInput() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; + double input = reference * 2; + double delta = Math.abs(input - reference) / 2.0; + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + double overshoot = Math.abs(input - reference) - delta; + + double expectedClip = input + overshoot; + + assertTrue(input < 0.0); + assertTrue(reference < 0.0); + assertTrue(input < reference); + assertTrue(variable.isLimitingActive()); + assertEquals(expectedClip, + variable.getDoubleValue(), + 1e-8, + "Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs( + input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot); + } + } + + @Test + public void testReferenceAndInputBothPositiveWithOvershootInputGreaterThanReference() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND); + double input = reference * 2; + double delta = Math.abs(input - reference) / 2.0; + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + double overshoot = Math.abs(input - reference) - delta; + + double expectedClip = input - overshoot; + + assertTrue(input > 0.0); + assertTrue(reference > 0.0); + assertTrue(input > reference); + assertTrue(variable.isLimitingActive()); + assertEquals(expectedClip, + variable.getDoubleValue(), + 1e-8, + "Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs( + input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot); + } + } + + @Test + public void testReferenceAndInputBothPositiveWithOvershootReferenceGreaterThanInput() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND); + double input = reference / 2.0; + double delta = Math.abs(input - reference) / 2.0; + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + double overshoot = Math.abs(input - reference) - delta; + + double expectedClip = input + overshoot; + + assertTrue(input > 0.0); + assertTrue(reference > 0.0); + assertTrue(input < reference); + assertTrue(variable.isLimitingActive()); + assertEquals(expectedClip, + variable.getDoubleValue(), + 1e-8, + "Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs( + input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot); + } + } + + @Test + public void testPositiveReferenceNegativeInputWithOvershoot() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND); + double input = reference * -0.5; + double delta = Math.abs(input - reference) / 2.0; + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + double overshoot = Math.abs(input - reference) - delta; + + double expectedClip = input + overshoot; + + assertTrue(input < 0.0); + assertTrue(reference > 0.0); + assertTrue(input < reference); + assertTrue(variable.isLimitingActive()); + assertEquals(expectedClip, + variable.getDoubleValue(), + 1e-8, + "Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs( + input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot); + } + } + + @Test + public void testNegativeReferencePositiveInputWithOvershoot() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; + double input = reference * -0.5; + double delta = Math.abs(input - reference) / 2.0; + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + double overshoot = Math.abs(input - reference) - delta; + + double expectedClip = input - overshoot; + + assertTrue(input > 0.0); + assertTrue(reference < 0.0); + assertTrue(input > reference); + assertTrue(variable.isLimitingActive()); + assertEquals(expectedClip, + variable.getDoubleValue(), + 1e-8, + "Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs( + input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot); + } + } + + @Test + public void testOvershootThenNoOvershoot() + { + Random random = new Random(1976L); + registry = new YoRegistry("registry"); + variable = new DeltaLimitedYoVariable("testVar", registry, 0.0); + + for (int i = 0; i < 60000; i++) + { + double reference = RandomNumbers.nextInt(random, RANDOM_LOWER_BOUND, RANDOM_UPPER_BOUND) * -1.0; + double input = reference * -0.5; + double delta = Math.abs(input - reference) / 2.0; + + variable.setMaxDelta(delta); + variable.updateOutput(reference, input); + + double overshoot = Math.abs(input - reference) - delta; + + double expectedClip = input - overshoot; + + assertTrue(input > 0.0); + assertTrue(reference < 0.0); + assertTrue(input > reference); + assertTrue(variable.isLimitingActive()); + assertEquals(expectedClip, + variable.getDoubleValue(), + 1e-8, + "Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs( + input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot); + + input = variable.getDoubleValue(); + + variable.updateOutput(reference, input); + double newRequestedDelta = Math.abs(input - reference); + assertFalse(newRequestedDelta > delta); + assertFalse(variable.isLimitingActive()); + assertEquals(expectedClip, + variable.getDoubleValue(), + 1e-8, + "Variable not clipped correctly\nReference: " + reference + "\nInput: " + input + "\nMagnitude of requested delta: " + Math.abs( + input - reference) + "\nMax Allowed Delta: " + delta + "\nOvershoot: " + overshoot); + } + } +} \ No newline at end of file diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java new file mode 100644 index 00000000..ead6291e --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java @@ -0,0 +1,70 @@ +package us.ihmc.yoVariables.filters; + +import static org.junit.jupiter.api.Assertions.*; + +import org.junit.jupiter.api.Test; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class FilteredDiscreteVelocityYoVariableTest +{ + + private static final double DT = 0.1; + + @Test + public void testFilteredDiscreteVelocityNoDirectionChange() + { + double alpha = 0.99; + YoRegistry registry = new YoRegistry("testRegistry"); + YoDouble positionVariable = new YoDouble("positionVariable", registry); + YoDouble time = new YoDouble("time", registry); + FilteredDiscreteVelocityYoVariable filteredDiscreteVelocityYoVariable = new FilteredDiscreteVelocityYoVariable("filteredDiscreteVelocityYoVariable", "", + alpha, positionVariable, time, registry); + + positionVariable.set(10); + time.set(0); + + for (int i = 0; i < 1000 / DT; i++) + { + time.add(DT); + positionVariable.add(1); + filteredDiscreteVelocityYoVariable.update(); + } + + assertEquals(10, filteredDiscreteVelocityYoVariable.getDoubleValue(), 1e-7); + } + + @Test + public void testFilteredDiscreteVelocityWithDirectionChange() + { + double alpha = 0.99; + YoRegistry registry = new YoRegistry("testRegistry"); + YoDouble positionVariable = new YoDouble("positionVariable", registry); + YoDouble time = new YoDouble("time", registry); + FilteredDiscreteVelocityYoVariable filteredDiscreteVelocityYoVariable = new FilteredDiscreteVelocityYoVariable("filteredDiscreteVelocityYoVariable", "", + alpha, positionVariable, time, registry); + + positionVariable.set(10); + time.set(0); + + for (int i = 0; i < 1000 / DT; i++) + { + time.add(DT); + positionVariable.add(1); + filteredDiscreteVelocityYoVariable.update(); + } + + assertEquals(10, filteredDiscreteVelocityYoVariable.getDoubleValue(), 1e-7); + + for (int i = 0; i < 1000 / DT; i++) + { + time.add(DT); + positionVariable.add(-1); + filteredDiscreteVelocityYoVariable.update(); + } + + assertEquals(-10, filteredDiscreteVelocityYoVariable.getDoubleValue(), 1e-7); + } + +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredVelocityYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredVelocityYoVariableTest.java new file mode 100644 index 00000000..fe06ca87 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredVelocityYoVariableTest.java @@ -0,0 +1,67 @@ +package us.ihmc.yoVariables.filters; + +import org.junit.jupiter.api.Test; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +import static org.junit.jupiter.api.Assertions.*; + +public class FilteredVelocityYoVariableTest +{ + + private static final double DT = 0.1; + + @Test + public void testUpdateForTranslationalVelocity() + { + YoRegistry registry = new YoRegistry("testRegistry"); + double alpha = 0.3; + YoDouble positionVariable = new YoDouble("positionVariable", registry); + + FilteredFiniteDifferenceYoVariable filteredVelocityYoVariable = new FilteredFiniteDifferenceYoVariable("filteredVelocityYoVariable", "test description", alpha, + positionVariable, DT, registry); + + filteredVelocityYoVariable.set(0); + positionVariable.set(0); + + for (int i = 0; i < 10000; i++) + { + positionVariable.add(10); + filteredVelocityYoVariable.update(); + } + + assertEquals(100, filteredVelocityYoVariable.getDoubleValue(), 1e-7); + + } + + @Test + public void testUpdateForRotationalVelocity() + { + YoRegistry registry = new YoRegistry("testRegistry"); + double alpha = 0.005; + YoDouble positionVariable = new YoDouble("positionVariable", registry); + + FilteredFiniteDifferenceYoVariable filteredVelocityYoVariable = new FilteredFiniteDifferenceYoVariable("filteredVelocityYoVariable", "test description", alpha, + positionVariable, DT, registry); + + filteredVelocityYoVariable.set(0); + positionVariable.set(-Math.PI); + + for (int i = 0; i < 10000; i++) + { + if (positionVariable.getValueAsDouble() + 0.5 > Math.PI) + { + positionVariable.set(-Math.PI + (Math.PI - positionVariable.getValueAsDouble())); + } + else + { + positionVariable.add(.5); + } + + filteredVelocityYoVariable.updateForAngles(); + } + + assertEquals(5, filteredVelocityYoVariable.getDoubleValue(), 1e-5); + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java new file mode 100644 index 00000000..b97a1485 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java @@ -0,0 +1,239 @@ +package us.ihmc.yoVariables.filters; + +import org.junit.jupiter.api.Test; +import us.ihmc.yoVariables.filters.FirstOrderFilteredYoDouble.FirstOrderFilterType; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class FirstOrderFilteredYoDoubleTest +{ + private final YoRegistry registry = new YoRegistry("testRegistry"); + private final YoDouble yoTime = new YoDouble("yoTime", registry); + + private final double DT = 0.001; + + @Test + public void testHighPassAttenuationForSinusoidalInput() + { + double inputFrequencyRadPerSec = 15.0; + double cutoffFrequencyRadPerSec = inputFrequencyRadPerSec / 5.0; + double filterAttenuation = 1.0; + double properHighPassAttenuation; + + FirstOrderFilteredYoDouble highPassFilteredYoVariable = new FirstOrderFilteredYoDouble("highPass", + "", + cutoffFrequencyRadPerSec / (2.0 * Math.PI), + yoTime, + FirstOrderFilterType.HIGH_PASS, + registry); + + while (filterAttenuation > 0.1 && cutoffFrequencyRadPerSec > 0.0) + { + highPassFilteredYoVariable.setCutoffFrequencyHz(cutoffFrequencyRadPerSec / (2.0 * Math.PI)); + highPassFilteredYoVariable.reset(); + filterAttenuation = computeSteadyStateFilteredOutputAmplitude(yoTime, DT, inputFrequencyRadPerSec, highPassFilteredYoVariable); + + properHighPassAttenuation = computeProperHighPassAttenuation(inputFrequencyRadPerSec, cutoffFrequencyRadPerSec); + + assertEquals(properHighPassAttenuation, filterAttenuation, 1e-2); + + cutoffFrequencyRadPerSec += 10.0; + } + } + + @Test + public void testLowPassAttenuationForSinusoidalInput() + { + double inputFrequencyRadPerSec = 10.0; + double cutoffFrequencyRadPerSec = inputFrequencyRadPerSec * 5.0; + double filterAttenuation = 1.0; + double properLowPassAttenuation; + + FirstOrderFilteredYoDouble lowPassFilteredYoVariable = new FirstOrderFilteredYoDouble("lowPass", + "", + cutoffFrequencyRadPerSec / (2.0 * Math.PI), + yoTime, + FirstOrderFilterType.LOW_PASS, + registry); + + while (filterAttenuation > 0.1 && cutoffFrequencyRadPerSec > 0.0) + { + lowPassFilteredYoVariable.setCutoffFrequencyHz(cutoffFrequencyRadPerSec / (2.0 * Math.PI)); + lowPassFilteredYoVariable.reset(); + filterAttenuation = computeSteadyStateFilteredOutputAmplitude(yoTime, DT, inputFrequencyRadPerSec, lowPassFilteredYoVariable); + + properLowPassAttenuation = computeProperLowPassAttenuation(inputFrequencyRadPerSec, cutoffFrequencyRadPerSec); + + assertEquals(properLowPassAttenuation, filterAttenuation, 1e-2); + + cutoffFrequencyRadPerSec -= 10.0; + } + } + + @Test + public void testBandPassAttenuationForSinusoidalInput() + { + double inputFrequencyRadPerSec = 10.0; + + double a = inputFrequencyRadPerSec / 5.0; + double b = inputFrequencyRadPerSec * 5.0; + + double filterAttenuation = 1.0; + double properBandPassAttenuation; + + FirstOrderBandPassFilteredYoDouble bandPassFilteredYoVariable = new FirstOrderBandPassFilteredYoDouble("sineWave", + "", + a, + b, + yoTime, + FirstOrderBandPassFilteredYoDouble.FirstOrderFilterType.BAND, + registry); + + while (filterAttenuation > 0.1 && a > 0.0 && b > 0.0) + { + bandPassFilteredYoVariable.setPassBand(a / (2.0 * Math.PI), b / (2.0 * Math.PI)); + bandPassFilteredYoVariable.reset(); + filterAttenuation = computeSteadyStateFilteredOutputAmplitude(yoTime, DT, inputFrequencyRadPerSec, bandPassFilteredYoVariable); + + properBandPassAttenuation = computeProperBandPassAttenuation(inputFrequencyRadPerSec, a, b); + + assertEquals(properBandPassAttenuation, filterAttenuation, 1e-2); + + a -= 10.0; + b -= 10.0; + } + } + + private double computeProperLowPassAttenuation(double inputFreq_RadPerSec, double cutoffFreq_RadPerSec) + { + double ret = cutoffFreq_RadPerSec / Math.sqrt(inputFreq_RadPerSec * inputFreq_RadPerSec + cutoffFreq_RadPerSec * cutoffFreq_RadPerSec); + return ret; + } + + private double computeProperHighPassAttenuation(double inputFreq_RadPerSec, double cutoffFreq_RadPerSec) + { + double ret = inputFreq_RadPerSec / Math.sqrt(inputFreq_RadPerSec * inputFreq_RadPerSec + cutoffFreq_RadPerSec * cutoffFreq_RadPerSec); + return ret; + } + + private double computeProperBandPassAttenuation(double inputFreq_RadPerSec, double minFreq_RadPerSec, double maxFreq_RadPerSec) + { + double highPass = computeProperHighPassAttenuation(inputFreq_RadPerSec, minFreq_RadPerSec); + double lowPass = computeProperLowPassAttenuation(inputFreq_RadPerSec, maxFreq_RadPerSec); + + double ret = highPass * lowPass; + return ret; + } + + private double computeSteadyStateFilteredOutputAmplitude(YoDouble yoTime, + double DT, + double inputFrequencyRadPerSec, + FirstOrderFilteredYoDouble filteredYoVariable) + { + double t; + double sineWaveInput; + + double filterOutput_oldest = 0.0; + double filterOutput_old = 0.0; + double filterOutput; + + double filterOutputPeak = 0.0; + double filterOutputPeakOld = 0.0; + double filterOutputPeakPercentChange = 100.0; + + boolean filterOutputHasReachedSteadyState = false; + + int i = 0; + + while (!filterOutputHasReachedSteadyState) + { + t = i * DT; + yoTime.set(t); + + sineWaveInput = Math.sin(inputFrequencyRadPerSec * t); + + filteredYoVariable.update(sineWaveInput); + + filterOutput = filteredYoVariable.getDoubleValue(); + + boolean filterOutputJustHitAPeak = filterOutput_old > filterOutput_oldest && filterOutput_old > filterOutput; + + if (filterOutputJustHitAPeak) + { + filterOutputPeak = filterOutput_old; + filterOutputPeakPercentChange = 100.0 * Math.abs((filterOutputPeak - filterOutputPeakOld) / filterOutputPeak); + + filterOutputPeakOld = filterOutputPeak; + + // System.out.println("Filter output peak :" + filterOutputPeak); + } + + filterOutputHasReachedSteadyState = filterOutputPeakPercentChange < 1e-6; + + filterOutput_oldest = filterOutput_old; + filterOutput_old = filterOutput; + i++; + } + + // System.out.println("Max Filter Output Percent Change: " + filterOutputPeakPercentChange); + + return filterOutputPeak; + } + + private double computeSteadyStateFilteredOutputAmplitude(YoDouble yoTime, + double DT, + double inputFrequencyRadPerSec, + FirstOrderBandPassFilteredYoDouble filteredYoVariable) + { + double t; + double sineWaveInput; + + double filterOutput_oldest = 0.0; + double filterOutput_old = 0.0; + double filterOutput; + + double filterOutputPeak = 0.0; + double filterOutputPeakOld = 0.0; + double filterOutputPeakPercentChange = 100.0; + + boolean filterOutputHasReachedSteadyState = false; + + int i = 0; + + while (!filterOutputHasReachedSteadyState) + { + t = i * DT; + yoTime.set(t); + + sineWaveInput = Math.sin(inputFrequencyRadPerSec * t); + + filteredYoVariable.update(sineWaveInput); + + filterOutput = filteredYoVariable.getDoubleValue(); + + boolean filterOutputJustHitAPeak = filterOutput_old > filterOutput_oldest && filterOutput_old > filterOutput; + + if (filterOutputJustHitAPeak) + { + filterOutputPeak = filterOutput_old; + filterOutputPeakPercentChange = 100.0 * Math.abs((filterOutputPeak - filterOutputPeakOld) / filterOutputPeak); + + filterOutputPeakOld = filterOutputPeak; + + // System.out.println("Filter output peak :" + filterOutputPeak); + } + + filterOutputHasReachedSteadyState = filterOutputPeakPercentChange < 1e-6; + + filterOutput_oldest = filterOutput_old; + filterOutput_old = filterOutput; + i++; + } + + // System.out.println("Max Filter Output Percent Change: " + filterOutputPeakPercentChange); + + return filterOutputPeak; + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBooleanTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBooleanTest.java new file mode 100644 index 00000000..a19285a6 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBooleanTest.java @@ -0,0 +1,152 @@ +package us.ihmc.yoVariables.filters; + + +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoBoolean; + +import static org.junit.jupiter.api.Assertions.*; + +public class GlitchFilteredYoBooleanTest +{ + public static final int WINDOW_SIZE = 10; + private YoRegistry registry; + private YoBoolean yoVariableToFilter; + private GlitchFilteredYoBoolean filteredVariable; + + @BeforeEach + public void setUp() + { + registry = new YoRegistry("testRegistry"); + yoVariableToFilter = new YoBoolean("variableToFilter", registry); + filteredVariable = new GlitchFilteredYoBoolean("filteredVariable", registry, yoVariableToFilter, WINDOW_SIZE); + } + + @AfterEach + public void tearDown() + { + registry = null; + yoVariableToFilter = null; + filteredVariable = null; + } + + @Test + public void testConstructors_Set_Get() + { + GlitchFilteredYoBoolean number1 = new GlitchFilteredYoBoolean("stringInt", new YoRegistry("test"), WINDOW_SIZE); + GlitchFilteredYoBoolean number2 = new GlitchFilteredYoBoolean("stringYoVariableRegistryInt", registry, WINDOW_SIZE); + GlitchFilteredYoBoolean number3 = new GlitchFilteredYoBoolean("stringBooleanYoVariableInt", new YoRegistry("test"), yoVariableToFilter, WINDOW_SIZE); + GlitchFilteredYoBoolean number4 = new GlitchFilteredYoBoolean("stringYoVariableRegistryBooleanYoVariableInt", registry, + yoVariableToFilter, WINDOW_SIZE); + + GlitchFilteredYoBoolean array[] = { number1, number2, number3, number4 }; + + for (int i = 0; i < array.length; i++) + { + assertFalse(array[i].getBooleanValue()); + //assertFalse(array[i].getPreviousBooleanValue()); + + array[i].set(true); + assertTrue(array[i].getBooleanValue()); + //assertTrue(array[i].getPreviousBooleanValue()); + + array[i].set(false); + assertFalse(array[i].getBooleanValue()); + //assertFalse(array[i].getPreviousBooleanValue()); + } + } + + @Test + public void testUpdate() + { + int windowSize = 3; + filteredVariable.set(true); + filteredVariable.setWindowSize(windowSize); + int numberOfSets = windowSize * 20; + +// for (int i = 0; i < numberOfSets; i++) +// { +// if (i % 2 == 0) +// { +// filteredVariable.update(false); +// assertFalse(filteredVariable.getPreviousBooleanValue()); +// } +// else +// { +// filteredVariable.update(true); +// assertTrue(filteredVariable.getPreviousBooleanValue()); +// } +// assertTrue(filteredVariable.getBooleanValue()); +// } + + + int counter = 0; + for (int i = 0; i < (windowSize + 10); i++) + { + + filteredVariable.update(false); + counter++; + + //assertFalse(filteredVariable.getPreviousBooleanValue()); + + if (counter < windowSize ) + { + assertTrue(filteredVariable.getBooleanValue()); + } + else + { + assertFalse(filteredVariable.getBooleanValue()); + } + } + + filteredVariable = null; + try + { + filteredVariable.update(); + fail(); + } + catch (RuntimeException rte) + { + //do nothing + } + } + + @Test + public void testCounter() + { + int windowSize = 10; + filteredVariable.set(true); + filteredVariable.setWindowSize(windowSize); + + + for (int i = 0; i < ( (int)(windowSize/2.0)); i++) + { + filteredVariable.update(filteredVariable.getBooleanValue()); + assertEquals(filteredVariable.counter.getIntegerValue(), 0); + } + + } + + @Test + public void testFiltering() + { + yoVariableToFilter.set(true); + + for (int i = 0; i < WINDOW_SIZE / 2; i++) + { + filteredVariable.update(); + } + + assertFalse(yoVariableToFilter.getBooleanValue() == filteredVariable.getBooleanValue()); + + for (int i = 0; i < WINDOW_SIZE / 2; i++) + { + filteredVariable.update(); + } + + assertTrue(yoVariableToFilter.getBooleanValue() == filteredVariable.getBooleanValue()); + } +} \ No newline at end of file diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java new file mode 100644 index 00000000..4975fecd --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java @@ -0,0 +1,71 @@ +package us.ihmc.yoVariables.filters; + +import static org.junit.jupiter.api.Assertions.*; + +import java.util.Random; + +import org.junit.jupiter.api.Test; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +public class MovingAverageYoDoubleTest +{ + private final Random rng = new Random(); + + @Test + public void testBetaFilteredYoVariable() + { + int beta = 5000; + double pseudoNoise = 0; + + YoRegistry registry = new YoRegistry("testRegistry"); + YoDouble positionVariable = new YoDouble("positionVariable", registry); + MovingAverageYoDouble betaFilteredYoVariable = new MovingAverageYoDouble("betaFilteredYoVariable", registry, beta, positionVariable); + + positionVariable.set(10); + + for (int i = 0; i < 10000; i++) + { + if (i % 2 == 0) + { + pseudoNoise = rng.nextDouble(); + } + positionVariable.add(Math.pow(-1, i) * pseudoNoise); + betaFilteredYoVariable.update(); + } + + assertEquals(10, betaFilteredYoVariable.getDoubleValue(), 1); + } + + @Test + public void testTrueMovingAverage() + { + int beta = 10; + + YoRegistry registry = new YoRegistry("testRegistry"); + MovingAverageYoDouble betaFilteredYoVariable = new MovingAverageYoDouble("betaFilteredYoVariable", registry, beta); + + double epsilon = 1e-10; + + betaFilteredYoVariable.update(1.0); + assertEquals(1.0, betaFilteredYoVariable.getDoubleValue(), epsilon); + + betaFilteredYoVariable.update(2.0); + assertEquals(1.5, betaFilteredYoVariable.getDoubleValue(), epsilon); + + + betaFilteredYoVariable.update(3.0); + betaFilteredYoVariable.update(4.0); + betaFilteredYoVariable.update(5.0); + betaFilteredYoVariable.update(6.0); + betaFilteredYoVariable.update(7.0); + betaFilteredYoVariable.update(8.0); + betaFilteredYoVariable.update(9.0); + betaFilteredYoVariable.update(10.0); + + assertEquals(5.5, betaFilteredYoVariable.getDoubleValue(), epsilon); + } + + +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RateLimitedYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RateLimitedYoVariableTest.java new file mode 100644 index 00000000..f437422f --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RateLimitedYoVariableTest.java @@ -0,0 +1,183 @@ +package us.ihmc.yoVariables.filters; + +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +import static org.junit.jupiter.api.Assertions.*; + +public class RateLimitedYoVariableTest +{ + YoRegistry registry = new YoRegistry("registry"); + RateLimitedYoVariable rateLimitedYoVariable1, rateLimitedYoVariable2, rateLimitedYoVariable3, rateLimitedYoVariable4; + YoDouble maxRate2, maxRate4; + YoDouble position3, position4; + double maxRate1, maxRate3; + double dt1, dt2, dt3, dt4; + + @BeforeEach + public void setUp() + { + maxRate2 = new YoDouble("maxRate2", registry); + maxRate4 = new YoDouble("maxRate4", registry); + position3 = new YoDouble("position3", registry); + position4 = new YoDouble("position4", registry); + + maxRate1 = 10.0; + maxRate2.set(9.0); + maxRate3 = 11.0; + maxRate4.set(12.0); + + dt1 = 1.0; + dt2 = 1.0; + dt3 = 1.0; + dt4 = 1.0; + + position3.set(0.5); + position4.set(0.75); + + rateLimitedYoVariable1 = new RateLimitedYoVariable("rateLimitedYoVariable1", registry, maxRate1, dt1); + rateLimitedYoVariable2 = new RateLimitedYoVariable("rateLimitedYoVariable2", registry, maxRate2, dt2); + rateLimitedYoVariable3 = new RateLimitedYoVariable("rateLimitedYoVariable3", registry, maxRate3, position3, dt3); + rateLimitedYoVariable4 = new RateLimitedYoVariable("rateLimitedYoVariable4", registry, maxRate4, position4, dt4); + } + + @Test + public void testUpdate() + { + try + { + rateLimitedYoVariable3.update(); + rateLimitedYoVariable4.update(); + } + catch (Exception e) + { + fail(); + } + } + + @Test + public void testUpdateWithNullPointerException() + { + try + { + rateLimitedYoVariable1.update(); + rateLimitedYoVariable2.update(); + + fail("Did not throw NullPointerException."); + } + catch (Exception e) + { + + } + } + + @Test + public void testUpdateWithCurrentPositionParameter() + { + for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) + { + double currentPosition1 = 10.0 * Math.sin(angle); + rateLimitedYoVariable1.update(currentPosition1); + assertEquals(rateLimitedYoVariable1.getDoubleValue(), currentPosition1, 1E-13); + } + + for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) + { + double currentPosition2 = 7.0 * Math.sin(angle); + rateLimitedYoVariable2.update(currentPosition2); + assertEquals(rateLimitedYoVariable2.getDoubleValue(), currentPosition2, 1E-13); + } + + for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) + { + double currentPosition3 = 11.0 * Math.sin(angle); + rateLimitedYoVariable3.update(currentPosition3); + assertEquals(rateLimitedYoVariable3.getDoubleValue(), currentPosition3, 1E-13); + } + + for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) + { + double currentPosition4 = 12.0 * Math.sin(angle); + rateLimitedYoVariable4.update(currentPosition4); + assertEquals(rateLimitedYoVariable4.getDoubleValue(), currentPosition4, 1E-13); + } + } + + @Test + public void testUpdateWithCurrentPositionParameterExceedingMaxRate() + { + for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) + { + double currentPosition1 = 25.0 * Math.sin(angle); + double dSinTheta = Math.cos(angle); + double signOfSlope = Math.signum(dSinTheta) * 1.0; + + if (Math.abs(currentPosition1 - rateLimitedYoVariable1.getDoubleValue()) > maxRate1) + { + currentPosition1 = rateLimitedYoVariable1.getDoubleValue() + signOfSlope * maxRate1; + } + rateLimitedYoVariable1.update(currentPosition1); + assertEquals(rateLimitedYoVariable1.getDoubleValue(), currentPosition1, 1E-15); + } + + for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) + { + double currentPosition2 = 25.0 * Math.sin(angle); + double dSinTheta = Math.cos(angle); + double signOfSlope = Math.signum(dSinTheta) * 1.0; + + if (Math.abs(currentPosition2 - rateLimitedYoVariable2.getDoubleValue()) > maxRate2.getDoubleValue()) + { + currentPosition2 = rateLimitedYoVariable2.getDoubleValue() + signOfSlope * maxRate2.getDoubleValue(); + } + rateLimitedYoVariable2.update(currentPosition2); + assertEquals(rateLimitedYoVariable2.getDoubleValue(), currentPosition2, 1E-15); + } + + for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) + { + double currentPosition3 = 25.0 * Math.sin(angle); + double dSinTheta = Math.cos(angle); + double signOfSlope = Math.signum(dSinTheta) * 1.0; + + if (Math.abs(currentPosition3 - rateLimitedYoVariable3.getDoubleValue()) > maxRate3) + { + currentPosition3 = rateLimitedYoVariable3.getDoubleValue() + signOfSlope * maxRate3; + } + rateLimitedYoVariable3.update(currentPosition3); + assertEquals(rateLimitedYoVariable3.getDoubleValue(), currentPosition3, 1E-15); + } + + for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) + { + double currentPosition4 = 25.0 * Math.sin(angle); + double dSinTheta = Math.cos(angle); + double signOfSlope = Math.signum(dSinTheta) * 1.0; + + if (Math.abs(currentPosition4 - rateLimitedYoVariable4.getDoubleValue()) > maxRate4.getDoubleValue()) + { + currentPosition4 = rateLimitedYoVariable4.getDoubleValue() + signOfSlope * maxRate4.getDoubleValue(); + } + rateLimitedYoVariable4.update(currentPosition4); + assertEquals(rateLimitedYoVariable4.getDoubleValue(), currentPosition4, 1E-15); + } + } + + @Test + public void testUpdateWithMaxRateBeingNegative() + { + try + { + RateLimitedYoVariable rateLimitedYoVariableWithNegativeMaxRate = new RateLimitedYoVariable("rateLimitedYoVariableWithNegativeMaxRate", registry, -5.0, + 1.0); + rateLimitedYoVariableWithNegativeMaxRate.update(5.0); + } + catch (RuntimeException e) + { + assertEquals(e.getMessage(), "The maxRate parameter in the RateLimitedYoVariable cannot be negative."); + } + } +} \ No newline at end of file diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RunningAverageYoDoubleTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RunningAverageYoDoubleTest.java new file mode 100644 index 00000000..8b9909ee --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RunningAverageYoDoubleTest.java @@ -0,0 +1,29 @@ +package us.ihmc.yoVariables.filters; + +import java.util.Random; + +import org.apache.commons.math3.stat.descriptive.moment.Mean; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; + +public class RunningAverageYoDoubleTest +{ + @Test + public void testAgainstApacheMean() + { + Random random = new Random(1); + RunningAverageYoDouble yoAverage = new RunningAverageYoDouble("", null); + Mean mean = new Mean(); + double next = -10.0; + + for (int i = 0; i < 1000; i++) + { + next += random.nextDouble(); + yoAverage.update(next); + mean.increment(next); + + Assertions.assertEquals(mean.getResult(), yoAverage.getValue()); + Assertions.assertEquals(mean.getN(), yoAverage.getSampleSize()); + } + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java new file mode 100644 index 00000000..93acfeaa --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java @@ -0,0 +1,68 @@ +package us.ihmc.yoVariables.filters; + +import org.junit.jupiter.api.Test; +import us.ihmc.yoVariables.registry.YoRegistry; + +import static org.junit.jupiter.api.Assertions.*; + +public class SecondOrderFilteredYoDoubleTest +{ + YoRegistry registry = new YoRegistry(getClass().getSimpleName()); + + @Test + public void testLowPassFilterCoefficients() + { + double dt = 0.001; + double dampingRatio = 1.0; + double naturalFrequencyInHz = 10.0; + double[] bAssert = {3947.8417604357433, 7895.6835208714865, 3947.8417604357433}; + double[] aAssert = {4255275.254047619, -7992104.316479129, 3752620.429473252}; + + SecondOrderFilteredYoDouble filteredYoVariable = new SecondOrderFilteredYoDouble("lowPass", registry, dt, naturalFrequencyInHz, dampingRatio, + SecondOrderFilterType.LOW_PASS); + + double[] b = new double[3]; + double[] a = new double[3]; + filteredYoVariable.getFilterCoefficients(b, a); + assertArrayEquals(b, bAssert, 1e-8); + assertArrayEquals(a, aAssert, 1e-8); + } + + @Test + public void testNotchFilterCoefficients() + { + double dt = 0.001; + double dampingRatio = 1.0; + double naturalFrequencyInHz = 10.0; + double[] bAssert = {4003947.8417604356, -7992104.316479129, 4003947.8417604356}; + double[] aAssert = {4255275.254047619, -7992104.316479129, 3752620.429473252}; + + SecondOrderFilteredYoDouble filteredYoVariable = new SecondOrderFilteredYoDouble("notch", registry, dt, naturalFrequencyInHz, dampingRatio, + SecondOrderFilterType.NOTCH); + + double[] b = new double[3]; + double[] a = new double[3]; + filteredYoVariable.getFilterCoefficients(b, a); + assertArrayEquals(b, bAssert, 1e-8); + assertArrayEquals(a, aAssert, 1e-8); + } + + @Test + public void testHighPassFilterCoefficients() + { + double dt = 0.001; + double dampingRatio = 1.0; + double naturalFrequencyInHz = 10.0; + double[] bAssert = {4000000.0, -8000000.0, 4000000.0}; + double[] aAssert = {4255275.254047619, -7992104.316479129, 3752620.429473252}; + + SecondOrderFilteredYoDouble filteredYoVariable = new SecondOrderFilteredYoDouble("highPass", registry, dt, naturalFrequencyInHz, dampingRatio, + SecondOrderFilterType.HIGH_PASS); + + double[] b = new double[3]; + double[] a = new double[3]; + filteredYoVariable.getFilterCoefficients(b, a); + assertArrayEquals(b, bAssert, 1e-8); + assertArrayEquals(a, aAssert, 1e-8); + } +} diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java new file mode 100644 index 00000000..a62aa785 --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java @@ -0,0 +1,39 @@ +package us.ihmc.yoVariables.filters; + +import java.util.Random; + +import org.junit.jupiter.api.Test; + +import us.ihmc.commons.RandomNumbers; +import us.ihmc.yoVariables.registry.YoRegistry; + +import static org.junit.jupiter.api.Assertions.*; + +public class SimpleMovingAverageFilteredYoVariableTest +{ + @Test + public void testWithFixedSizeDoubleArrays() + { + for (int i = 0; i < 100; i++) + { + YoRegistry registry = new YoRegistry("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); + } + } +} From dc4c81e332f5440b8ac95b78df7b0b9f16bed1ba Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Wed, 16 Oct 2024 16:20:45 -0500 Subject: [PATCH 40/47] did considerable package movement, but solidified how the extracted repos will be structured --- ihmc-yovariables-filters/build.gradle.kts | 1 + .../yoVariables/filters/AlphaFilterTools.java | 1 + .../filters/AlphaFilteredYoVariable.java | 1 + .../ihmc/yoVariables/filters/AngleTools.java | 399 ------------------ .../yoVariables/filters/DeadbandTools.java | 140 ------ .../filters/DeadbandedYoVariable.java | 1 + .../FilteredFiniteDifferenceYoVariable.java | 1 + .../RateLimitedYoFrameOrientationTest.java | 2 +- .../RateLimitedYoFrameQuaternionTest.java | 2 +- .../euclid/filters/YoMatrixTest.java | 229 ++++++++++ 10 files changed, 236 insertions(+), 541 deletions(-) delete mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AngleTools.java delete mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandTools.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java diff --git a/ihmc-yovariables-filters/build.gradle.kts b/ihmc-yovariables-filters/build.gradle.kts index c218255d..34ca6a3c 100644 --- a/ihmc-yovariables-filters/build.gradle.kts +++ b/ihmc-yovariables-filters/build.gradle.kts @@ -15,6 +15,7 @@ ihmc { mainDependencies { api("us.ihmc:ihmc-yovariables:0.12.2") api("org.ejml:ejml-ddense:0.39") + api("us.ihmc:ihmc-commons-utils:source") } testDependencies { diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java index 12215c30..a1c446b9 100644 --- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java @@ -1,5 +1,6 @@ package us.ihmc.yoVariables.filters; +import us.ihmc.commons.AngleTools; import us.ihmc.commons.MathTools; public class AlphaFilterTools diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java index 0f7841fa..0a0bfa32 100644 --- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java @@ -1,5 +1,6 @@ package us.ihmc.yoVariables.filters; +import us.ihmc.commons.AngleTools; import us.ihmc.commons.MathTools; import us.ihmc.euclid.tools.EuclidCoreTools; import us.ihmc.yoVariables.providers.DoubleProvider; diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AngleTools.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AngleTools.java deleted file mode 100644 index e493d1d5..00000000 --- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AngleTools.java +++ /dev/null @@ -1,399 +0,0 @@ -package us.ihmc.yoVariables.filters; - -import us.ihmc.commons.MathTools; -import us.ihmc.euclid.referenceFrame.interfaces.FramePoint2DReadOnly; -import us.ihmc.euclid.referenceFrame.interfaces.FramePose2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DReadOnly; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.euclid.tuple4D.Quaternion32; - -import java.util.Random; - -// TODO replace with teh values that are pushed into ihmc commons -@Deprecated -public class AngleTools -{ - - public static final double PI = Math.PI; - public static final double TwoPI = 2.0 * PI; - public static final double EPSILON = 1e-10; - - private AngleTools() - { - - } - - public static double trimAngleMinusPiToPi(double angle) - { - return AngleTools.shiftAngleToStartOfRange(angle, -PI); - } - - public static float getAngle(Quaternion32 q) - { - return 2.0f * (float) Math.acos(q.getS()); - } - - public static double angleMinusPiToPi(Vector2DReadOnly startVector, Vector2DReadOnly endVector) - { - double absoluteAngle = Math.acos(startVector.dot(endVector) / startVector.norm() / endVector.norm()); - - Vector3D start3d = new Vector3D(startVector.getX(), startVector.getY(), 0.0); - Vector3D end3d = new Vector3D(endVector.getX(), endVector.getY(), 0.0); - - Vector3D crossProduct = new Vector3D(); - crossProduct.cross(start3d, end3d); - - if (crossProduct.getZ() >= 0.0) - { - return absoluteAngle; - } - else - { - return -absoluteAngle; - } - } - - public static double computeAngleAverage(double angleA, double angleB) - { - // average = A + (B-A)/2 = (A+B)/2 - return interpolateAngle(angleA, angleB, 0.5); - } - - /** - * Performs a linear interpolation from {@code angleA} to {@code angleB} given the percentage - * {@code alpha} and trim the result to be in [-pi, pi]. - *

    - *
  • If {@code alpha == 0}, the result is {@code angleA}. - *
  • If {@code alpha == 1}, the result is {@code angleB}. - *
  • If {@code alpha == 0.5}, the result is the average of the two angles. - *
  • The percentage {@code alpha} is not clamped to be in [0, 1] such that this method can be used - * for extrapolation. - *
- * - * @param angleA the first angle in the interpolation. - * @param angleB the second angle in the interpolation. - * @param alpha the percentage to use for the interpolation. A value of 0 will return - * {@code angleA}, while a value of 1 will return {@code angleB}. - * @return the interpolated angle in [-pi, pi]. - */ - public static double interpolateAngle(double angleA, double angleB, double alpha) - { - // A + alpha * (B-A)/2 - double average = angleA + alpha * computeAngleDifferenceMinusPiToPi(angleB, angleA); - return trimAngleMinusPiToPi(average); - } - - /** - * Formula found on Wikipedia. - */ - public static double computeAngleAverage(double[] angles) - { - double average = 0.0; - - double averageOfSin = 0.0; - double averageOfCos = 0.0; - double weight = 1.0 / angles.length; - - for (int i = 0; i < angles.length; i++) - { - averageOfSin += weight * Math.sin(angles[i]); - averageOfCos += weight * Math.cos(angles[i]); - } - - average = Math.atan2(averageOfSin, averageOfCos); - - return average; - } - - /** - * Finds the closest 90 degree yaw and returns number of 90 degrees (0 = 0; 1 = 90; 2 = 180; 3 = 270). - * - * @param yawInRadians double - * @return int between 0 and 3 for the number of 90 degree yawed. - */ - public static int findClosestNinetyDegreeYaw(double yawInRadians) - { - double minDifference = Double.POSITIVE_INFINITY; - int ret = -1; - - double difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, 0.0); - - minDifference = Math.abs(difference); - ret = 0; - - difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, Math.PI / 2.0); - - if (Math.abs(difference) < minDifference) - { - minDifference = Math.abs(difference); - ret = 1; - } - - difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, Math.PI); - - if (Math.abs(difference) < minDifference) - { - minDifference = Math.abs(difference); - ret = 2; - } - - difference = AngleTools.computeAngleDifferenceMinusPiToPi(yawInRadians, 3.0 * Math.PI / 2.0); - - if (Math.abs(difference) < minDifference) - { - minDifference = Math.abs(difference); - ret = 3; - } - - return ret; - } - - /** - * computeAngleDifferenceMinusPiToPi: returns (angleA - angleB), where the return value is [-pi, pi) - * - * @param angleA double - * @param angleB double - * @return double - */ - public static double computeAngleDifferenceMinusPiToPi(double angleA, double angleB) - { - double difference = angleA - angleB; - difference = difference % TwoPI; - difference = AngleTools.shiftAngleToStartOfRange(difference, -PI); - - return difference; - } - - /** - * computeAngleDifferenceMinusPiToPi: returns (angleA - angleB), where the return value is [-2.0*pi, 0.0) - * - * @param angleA double - * @param angleB double - * @return double - */ - public static double computeAngleDifferenceMinusTwoPiToZero(double angleA, double angleB) - { - double difference = angleA - angleB; - difference = difference % TwoPI; - difference = AngleTools.shiftAngleToStartOfRange(difference, -TwoPI); - - return difference; - } - - /** - * This will shift an angle to be in the range [startOfAngleRange, - * (startOfAngleRange + 2*pi) - * - * @param angleToShift the angle to shift - * @param startOfAngleRange start of the range. - * @return the shifted angle - */ - public static double shiftAngleToStartOfRange(double angleToShift, double startOfAngleRange) - { - return shiftAngleToStartOfRange(angleToShift, startOfAngleRange, TwoPI); - } - - /** - * This will shift an angle to be in the range [startOfAngleRange, - * (startOfAngleRange + endOfAngleRange) - * - * @param angleToShift the angle to shift - * @param startOfAngleRange start of the range. - * @param endOfAngleRange end of the range. - * @return the shifted angle - */ - public static double shiftAngleToStartOfRange(double angleToShift, double startOfAngleRange, double endOfAngleRange) - { - double ret = angleToShift; - startOfAngleRange = startOfAngleRange - EPSILON; - - if (angleToShift < startOfAngleRange) - { - ret = angleToShift + Math.ceil((startOfAngleRange - angleToShift) / endOfAngleRange) * endOfAngleRange; - } - - if (angleToShift >= (startOfAngleRange + endOfAngleRange)) - { - ret = angleToShift - Math.floor((angleToShift - startOfAngleRange) / endOfAngleRange) * endOfAngleRange; - } - - return ret; - } - - /** - * Returns an evenly distributed random number between -2PI and 2PI - * - * @param random Random number generator - * @return number between -2PI and 2PI - */ - public static double generateRandomAngle(Random random) - { - return -2.0 * Math.PI + 4 * Math.PI * random.nextDouble(); - } - - /** - * Returns array of angles increasing from -2PI to 2PI - * - * @param numberOfAngles - * @param stayThisFarAwayFromPlusMinus2PI - * @param includeZero - * @param includePlusMinusPI - * @return - */ - public static double[] generateArrayOfTestAngles(int numberOfAngles, double stayThisFarAwayFromPlusMinus2PI, boolean includeZero, boolean includePlusMinusPI) - { - int arraySize = numberOfAngles; - double epsilon = stayThisFarAwayFromPlusMinus2PI; - - if (includeZero && !includePlusMinusPI) - { - arraySize += 1; - } - else if (includePlusMinusPI && !includeZero) - { - arraySize += 2; - } - else if (includeZero && includePlusMinusPI) - { - arraySize += 3; - } - - double thetaMin = -2.0 * Math.PI + stayThisFarAwayFromPlusMinus2PI; - double thetaMax = 2.0 * Math.PI - stayThisFarAwayFromPlusMinus2PI; - double deltaTheta = Math.abs(thetaMax - thetaMin) / (numberOfAngles - 1); - - double[] ret = new double[arraySize]; - - for (int i = 0; i < numberOfAngles; i++) - { - ret[i] = thetaMin + deltaTheta * i; - - boolean epsilonEqualToZero = MathTools.epsilonEquals(Math.abs(ret[i]), 0.0, epsilon); - boolean epsilonEqualToPlusMinusPI = MathTools.epsilonEquals(Math.abs(ret[i]), Math.PI, epsilon); - - if (epsilonEqualToZero && !includeZero || epsilonEqualToPlusMinusPI && !includePlusMinusPI) - ret[i] += Math.signum(ret[i]) * Math.max(1.0e-4, Math.abs(epsilon)); - } - - if (includeZero && !includePlusMinusPI) - { - ret[numberOfAngles] = 0.0; - } - else if (includePlusMinusPI && !includeZero) - { - ret[numberOfAngles] = -Math.PI; - ret[numberOfAngles + 1] = Math.PI; - } - else if (includeZero && includePlusMinusPI) - { - ret[numberOfAngles] = 0.0; - ret[numberOfAngles + 1] = -Math.PI; - ret[numberOfAngles + 2] = Math.PI; - } - - return ret; - } - - /** - * Returns an angle between two points + heading Offset from -PI to PI. - * If the x or y components are both under the noTranslationTolerance, - * then the initial orientation as given in startPose will be returned. - * - * @param startPose initial position and orientation - * @param endPoint end position - * @param headingOffset offset from path angle - * @param noTranslationTolerance tolerance for determining if path angle should be determined - * @return number between -PI and PI - */ - public static double calculateHeading(FramePose2DReadOnly startPose, FramePoint2DReadOnly endPoint, double headingOffset, double noTranslationTolerance) - { - double deltaX = endPoint.getX() - startPose.getX(); - double deltaY = endPoint.getY() - startPose.getY(); - double heading; - if (Math.abs(deltaX) < noTranslationTolerance && Math.abs(deltaY) < noTranslationTolerance) - { - heading = startPose.getYaw(); - } - else - { - double pathHeading = Math.atan2(deltaY, deltaX); - heading = AngleTools.trimAngleMinusPiToPi(pathHeading + headingOffset); - } - return heading; - } - - /** - * Returns an angle between two points + heading Offset from -PI to PI. - * - * @param startPose initial position and orientation - * @param endPoint end position - * @param headingOffset offset from path angle - * @return number between -PI and PI - */ - public static double calculateHeading(Tuple3DReadOnly startPose, Tuple3DReadOnly endPoint, double headingOffset) - { - double deltaX = endPoint.getX() - startPose.getX(); - double deltaY = endPoint.getY() - startPose.getY(); - - double pathHeading = Math.atan2(deltaY, deltaX); - return AngleTools.trimAngleMinusPiToPi(pathHeading + headingOffset); - } - - /** - * Returns an angle between two points + heading Offset from -PI to PI. - * - * @param startPose initial position - * @param endPoint end position - * @return number between -PI and PI - */ - public static double calculateHeading(Point2DReadOnly startPose, Point2DReadOnly endPoint) - { - double deltaX = endPoint.getX() - startPose.getX(); - double deltaY = endPoint.getY() - startPose.getY(); - double heading; - - double pathHeading = Math.atan2(deltaY, deltaX); - heading = AngleTools.trimAngleMinusPiToPi(pathHeading); - - return heading; - } - - /** - * Pass in a vector. Get its angle in polar coordinates. - * - * @param vx - * @param vy - * @return angle of vector from 0 to 2PI - */ - public static double angleFromZeroToTwoPi(double vx, double vy) - { - double angleFromNegtivePiToPi = Math.atan2(vy, vx); - - if (angleFromNegtivePiToPi < 0.0) - { - return 2.0 * Math.PI + angleFromNegtivePiToPi; - } - else - { - return angleFromNegtivePiToPi; - } - } - - public static double roundToGivenPrecisionForAngle(double angleValue, double precisionFactor) - { - double centeredAngleValue = trimAngleMinusPiToPi(angleValue + 0.5 * precisionFactor); - long longValue = (long) (centeredAngleValue / precisionFactor); - double roundedValue = ((double) longValue) * precisionFactor; - return trimAngleMinusPiToPi(roundedValue); - } - - public static void roundToGivenPrecisionForAngles(Tuple3DBasics tuple3d, double precision) - { - tuple3d.setX(roundToGivenPrecisionForAngle(tuple3d.getX(), precision)); - tuple3d.setY(roundToGivenPrecisionForAngle(tuple3d.getY(), precision)); - tuple3d.setZ(roundToGivenPrecisionForAngle(tuple3d.getZ(), precision)); - } -} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandTools.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandTools.java deleted file mode 100644 index 98be17ac..00000000 --- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandTools.java +++ /dev/null @@ -1,140 +0,0 @@ -package us.ihmc.yoVariables.filters; - -import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics; -import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly; -import us.ihmc.euclid.tuple2D.interfaces.Vector2DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Point3DBasics; -import us.ihmc.euclid.tuple3D.interfaces.Point3DReadOnly; -import us.ihmc.euclid.tuple3D.interfaces.Vector3DBasics; - -// TODO replace with teh values that are pushed into ihmc commons -@Deprecated -public class DeadbandTools -{ - /** - * Applies a deadband of size {@param deadbandSize} to value {@param value}, centered about zero. - *

- * For example, if a deadband of 1 is applied to a signal of 3, the return is 2. If a deadband of 3 is applied to a signal of 3, the return is 0. - *

- * - * @param deadbandSize size of deadband to apply - * @param value value to apply the deadband to. - * @return value after applying the deadband. - */ - public static double applyDeadband(double deadbandSize, double value) - { - return applyDeadband(deadbandSize, 0.0, value); - } - - /** - * Applies a deadband of size {@param deadbandSize} to value {@param value}, centered about {@param deadbandCenter}. - *

- * For example, if a deadband of 1 is applied to a signal of 3 about 2, the return is 2. if a deadband of 3 is applied to a signal of 3 about 1, the return - * is 1. - *

- * - * @param deadbandSize size of deadband to apply - * @param deadbandCenter center about which the deadband is applied. - * @param value value to apply the deadband to. - * @return value after applying the deadband. - */ - public static double applyDeadband(double deadbandSize, double deadbandCenter, double value) - { - if (value > deadbandCenter) - return Math.max(deadbandCenter, value - deadbandSize); - else - return Math.min(deadbandCenter, value + deadbandSize); - } - - /** - * Applies a deadband to the length of a vector. - * - * @param vectorToPack vector to apply the deadband to. - * @param deadband deadband magnitude to apply. - */ - public static void applyDeadband(Vector2DBasics vectorToPack, double deadband) - { - double length = vectorToPack.norm(); - if (length < deadband) - { - vectorToPack.setToZero(); - } - else - { - double newLength = length - deadband; - vectorToPack.scale(newLength / length); - } - } - - /** - * Applies a deadband to the length of a vector. - * - * @param vectorToPack vector to apply the deadband to. - * @param deadband deadband magnitude to apply. - */ - public static boolean applyDeadband(Vector3DBasics vectorToPack, double deadband) - { - double length = vectorToPack.norm(); - if (length < deadband) - { - vectorToPack.setToZero(); - return false; - } - else - { - double newLength = length - deadband; - vectorToPack.scale(newLength / length); - return true; - } - } - - /** - * Applies a deadband to the magnitude of a point from another point, where the total distance is the value that is deadbanded. This can be thought of as - * applying a deadband of the size {@param deadband} to the vector from {@param centerPoint} to {@param pointToPack}, and then using this deadbanded vector - * to recompute the value of {@param pointToPack}. - * - * @param pointToPack point to apply the deadband to. - * @param centerPoint point about which to apply the deadband. - * @param deadband deadband magnitude to apply. - */ - public static boolean applyDeadband(Point2DBasics pointToPack, Point2DReadOnly centerPoint, double deadband) - { - double distance = pointToPack.distance(centerPoint); - if (distance < deadband) - { - pointToPack.set(centerPoint); - return false; - } - else - { - double newDistance = distance - deadband; - pointToPack.interpolate(centerPoint, 1.0 - newDistance / distance); - return true; - } - } - - /** - * Applies a deadband to the magnitude of a point from another point, where the total distance is the value that is deadbanded. This can be thought of as - * applying a deadband of the size {@param deadband} to the vector from {@param centerPoint} to {@param pointToPack}, and then using this deadbanded vector - * to recompute the value of {@param pointToPack}. - * - * @param pointToPack point to apply the deadband to. - * @param centerPoint point about which to apply the deadband. - * @param deadband deadband magnitude to apply. - */ - public static boolean applyDeadband(Point3DBasics pointToPack, Point3DReadOnly centerPoint, double deadband) - { - double distance = pointToPack.distance(centerPoint); - if (distance < deadband) - { - pointToPack.set(centerPoint); - return false; - } - else - { - double newDistance = distance - deadband; - pointToPack.interpolate(centerPoint, 1.0 - newDistance / distance); - return true; - } - } -} diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java index 12670240..0ec1cf4a 100644 --- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java @@ -1,5 +1,6 @@ package us.ihmc.yoVariables.filters; +import us.ihmc.commons.DeadbandTools; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java index 5c0db233..c0ac7d96 100644 --- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java @@ -1,5 +1,6 @@ package us.ihmc.yoVariables.filters; +import us.ihmc.commons.AngleTools; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoBoolean; diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java index f67bfce0..b2a83fdd 100644 --- a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java @@ -6,13 +6,13 @@ import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.Test; +import us.ihmc.commons.AngleTools; import us.ihmc.commons.RandomNumbers; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; import us.ihmc.euclid.tools.EuclidCoreRandomTools; import us.ihmc.euclid.tools.EuclidCoreTestTools; import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.yoVariables.filters.AngleTools; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java index f40e68d9..71fd7cce 100644 --- a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java @@ -8,13 +8,13 @@ import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.Test; +import us.ihmc.commons.AngleTools; import us.ihmc.commons.RandomNumbers; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; import us.ihmc.euclid.tools.EuclidCoreRandomTools; import us.ihmc.euclid.tools.EuclidCoreTestTools; import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.yoVariables.filters.AngleTools; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java new file mode 100644 index 00000000..ab37128f --- /dev/null +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java @@ -0,0 +1,229 @@ +package us.ihmc.yoVariables.euclid.filters; + +import java.util.Random; + +import org.ejml.EjmlUnitTests; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.RandomMatrices_DDRM; +import org.junit.jupiter.api.Test; + +import us.ihmc.yoVariables.filters.YoMatrix; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.variable.YoDouble; + +import static org.junit.jupiter.api.Assertions.*; + +public class YoMatrixTest +{ + @Test + public void testSimpleYoMatrixExample() + { + int maxNumberOfRows = 4; + int maxNumberOfColumns = 8; + YoRegistry registry = new YoRegistry("testRegistry"); + YoMatrix yoMatrix = new YoMatrix("testMatrix", maxNumberOfRows, maxNumberOfColumns, registry); + assertEquals(maxNumberOfRows, yoMatrix.getNumRows()); + assertEquals(maxNumberOfColumns, yoMatrix.getNumCols()); + + DMatrixRMaj denseMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns); + yoMatrix.reshape(maxNumberOfRows, maxNumberOfColumns); + yoMatrix.zero(); + yoMatrix.get(denseMatrix); + + DMatrixRMaj zeroMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns); + EjmlUnitTests.assertEquals(zeroMatrix, denseMatrix, 1e-10); + + Random random = new Random(1984L); + + DMatrixRMaj randomMatrix = RandomMatrices_DDRM.rectangle(maxNumberOfRows, maxNumberOfColumns, random); + yoMatrix.set(randomMatrix); + + DMatrixRMaj checkMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns); + yoMatrix.get(checkMatrix); + + EjmlUnitTests.assertEquals(randomMatrix, checkMatrix, 1e-10); + + assertEquals(registry.findVariable(YoMatrix.getFieldName("testMatrix", 0, 0)).getValueAsDouble(), checkMatrix.get(0, 0), 1e-10); + } + + @Test + public void testYoMatrixDimensioning() + { + int maxNumberOfRows = 4; + int maxNumberOfColumns = 8; + String name = "testMatrix"; + + YoRegistry registry = new YoRegistry("testRegistry"); + YoMatrix yoMatrix = new YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry); + + int smallerRows = maxNumberOfRows - 2; + int smallerColumns = maxNumberOfColumns - 3; + DMatrixRMaj denseMatrix = new DMatrixRMaj(smallerRows, smallerColumns); + + try + { + yoMatrix.get(denseMatrix); + fail("Should throw an exception if the size isn't right!"); + } + catch (Exception e) + { + } + + yoMatrix.reshape(maxNumberOfRows, maxNumberOfColumns); + yoMatrix.zero(); + yoMatrix.getAndReshape(denseMatrix); + DMatrixRMaj zeroMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns); + EjmlUnitTests.assertEquals(zeroMatrix, denseMatrix, 1e-10); + assertEquals(maxNumberOfRows, denseMatrix.getNumRows()); + assertEquals(maxNumberOfColumns, denseMatrix.getNumCols()); + + assertEquals(maxNumberOfRows, yoMatrix.getNumRows()); + assertEquals(maxNumberOfColumns, yoMatrix.getNumCols()); + + Random random = new Random(1984L); + + DMatrixRMaj randomMatrix = RandomMatrices_DDRM.rectangle(maxNumberOfRows, maxNumberOfColumns, random); + yoMatrix.set(randomMatrix); + + DMatrixRMaj checkMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns); + yoMatrix.get(checkMatrix); + + EjmlUnitTests.assertEquals(randomMatrix, checkMatrix, 1e-10); + + DMatrixRMaj smallerMatrix = RandomMatrices_DDRM.rectangle(smallerRows, smallerColumns, random); + yoMatrix.set(smallerMatrix); + + assertEquals(smallerRows, smallerMatrix.getNumRows()); + assertEquals(smallerColumns, smallerMatrix.getNumCols()); + + assertEquals(smallerRows, yoMatrix.getNumRows()); + assertEquals(smallerColumns, yoMatrix.getNumCols()); + + DMatrixRMaj checkMatrix2 = new DMatrixRMaj(1, 1); + yoMatrix.getAndReshape(checkMatrix2); + + EjmlUnitTests.assertEquals(smallerMatrix, checkMatrix2, 1e-10); + + checkMatrixYoVariablesEqualsCheckMatrixAndOutsideValuesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, checkMatrix2, registry); + } + + @Test + public void testYoMatrixSetToZero() + { + int maxNumberOfRows = 4; + int maxNumberOfColumns = 8; + String name = "testMatrixForZero"; + + YoRegistry registry = new YoRegistry("testRegistry"); + YoMatrix yoMatrix = new YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry); + + Random random = new Random(1984L); + + DMatrixRMaj randomMatrix = RandomMatrices_DDRM.rectangle(maxNumberOfRows, maxNumberOfColumns, random); + yoMatrix.set(randomMatrix); + + int numberOfRows = 2; + int numberOfColumns = 6; + yoMatrix.reshape(numberOfRows, numberOfColumns); + yoMatrix.zero(); + + DMatrixRMaj zeroMatrix = new DMatrixRMaj(numberOfRows, numberOfColumns); + zeroMatrix.zero(); + checkMatrixYoVariablesEqualsCheckMatrixAndOutsideValuesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, zeroMatrix, registry); + } + + @Test + public void testYoMatrixSetTooBig() + { + int maxNumberOfRows = 4; + int maxNumberOfColumns = 8; + String name = "testMatrix"; + YoRegistry registry = new YoRegistry("testRegistry"); + YoMatrix yoMatrix = new YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry); + + DMatrixRMaj tooBigMatrix = new DMatrixRMaj(maxNumberOfRows + 1, maxNumberOfColumns); + + try + { + yoMatrix.set(tooBigMatrix); + fail("Too Big"); + } + catch (RuntimeException e) + { + } + + tooBigMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns + 1); + + try + { + yoMatrix.set(tooBigMatrix); + fail("Too Big"); + } + catch (RuntimeException e) + { + } + + // Test a 0 X Big Matrix + DMatrixRMaj okMatrix = new DMatrixRMaj(0, maxNumberOfColumns + 10); + yoMatrix.set(okMatrix); + assertMatrixYoVariablesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, registry); + + DMatrixRMaj checkMatrix = new DMatrixRMaj(1, 1); + yoMatrix.getAndReshape(checkMatrix); + + assertEquals(0, checkMatrix.getNumRows()); + assertEquals(maxNumberOfColumns + 10, checkMatrix.getNumCols()); + + // Test a Big X 0 Matrix + + okMatrix = new DMatrixRMaj(maxNumberOfRows + 10, 0); + yoMatrix.set(okMatrix); + assertMatrixYoVariablesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, registry); + + checkMatrix = new DMatrixRMaj(1, 1); + yoMatrix.getAndReshape(checkMatrix); + + assertEquals(maxNumberOfRows + 10, checkMatrix.getNumRows()); + assertEquals(0, checkMatrix.getNumCols()); + + } + + + private void checkMatrixYoVariablesEqualsCheckMatrixAndOutsideValuesAreNaN(String name, int maxNumberOfRows, int maxNumberOfColumns, DMatrixRMaj checkMatrix, YoRegistry registry) + { + int smallerRows = checkMatrix.getNumRows(); + int smallerColumns = checkMatrix.getNumCols(); + + // Make sure the values are correct, including values outside the range should be NaN: + for (int row = 0; row < maxNumberOfRows; row++) + { + for (int column = 0; column < maxNumberOfColumns; column++) + { + YoDouble variable = (YoDouble) registry.findVariable(YoMatrix.getFieldName(name, row, column)); + + if ((row < smallerRows) && (column < smallerColumns)) + { + assertEquals(checkMatrix.get(row, column), variable.getDoubleValue(), 1e-10); + } + else + { + assertTrue(Double.isNaN(variable.getDoubleValue()), "Values outside aren't NaN, instead are " + variable.getDoubleValue()); + } + + } + } + } + + private void assertMatrixYoVariablesAreNaN(String name, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) + { + for (int row = 0; row < maxNumberOfRows; row++) + { + for (int column = 0; column < maxNumberOfColumns; column++) + { + YoDouble variable = (YoDouble) registry.findVariable(YoMatrix.getFieldName(name, row, column)); + assertTrue(Double.isNaN(variable.getDoubleValue())); + } + } + } + +} From e87b4338703ee44404ab3eafcb7636ab30215959 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Thu, 17 Oct 2024 22:14:19 -0500 Subject: [PATCH 41/47] started refactoring some of the code to prep for the commons move set up the euclid update repo, including javadoc and pulling over the tests moved the mecano packages around set up a robotics tools project to become its own repo --- .../euclid/filters/AlphaFilteredYoFramePoint3DTest.java | 1 + .../euclid/filters/AlphaFilteredYoFrameQuaternionTest.java | 3 ++- .../euclid/filters/AlphaFilteredYoFrameVector3DTest.java | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java index abbe1fa5..103c0504 100644 --- a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java @@ -7,6 +7,7 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreMissingRandomTools; import us.ihmc.euclid.tools.EuclidCoreRandomTools; import us.ihmc.euclid.tools.EuclidCoreTestTools; import us.ihmc.euclid.tuple3D.Point3D; diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java index 3d3998fa..7962aac5 100644 --- a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java @@ -9,9 +9,10 @@ import us.ihmc.euclid.axisAngle.AxisAngle; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; import us.ihmc.euclid.tools.EuclidCoreTestTools; import us.ihmc.euclid.tuple4D.Quaternion; -import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tools.EuclidCoreMissingRandomTools; import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java index 3d4a6c3f..a2be79c3 100644 --- a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java +++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java @@ -7,6 +7,7 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreMissingRandomTools; import us.ihmc.euclid.tools.EuclidCoreRandomTools; import us.ihmc.euclid.tools.EuclidCoreTestTools; import us.ihmc.euclid.tuple3D.Point3D; From 2427604a1953dcd8eadb310b13f31bd61b329511 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Fri, 18 Oct 2024 23:07:00 -0500 Subject: [PATCH 42/47] set up the ihmc math library more properly, and worked to add some javadoc on some of the core classes did a bunch of testing and javadoc, and cleaned up polynomial3D --- .../SecondOrderFilteredYoFrameVector3D.java | 5 +- .../AccelerationLimitedYoVariable.java | 3 + .../filters/AlphaBetaFilteredYoVariable.java | 65 ++-- .../yoVariables/filters/AlphaFilterTools.java | 8 +- .../filters/AlphaFilteredYoMatrix.java | 20 -- .../ButterworthFilteredYoVariable.java | 152 +++++++++ .../FilteredDiscreteVelocityYoVariable.java | 5 - .../FirstOrderBandPassFilteredYoDouble.java | 7 +- .../filters/FirstOrderFilteredYoDouble.java | 2 +- .../filters/ProcessingYoVariable.java | 4 +- .../filters/RateLimitedYoVariable.java | 134 +++++++- .../filters/SecondOrderFilterType.java | 6 - .../filters/SecondOrderFilteredYoDouble.java | 5 + ...condOrderFilteredYoVariableParameters.java | 6 +- .../AlphaFilteredYoFramePoint3DTest.java | 1 - .../AlphaFilteredYoFrameQuaternionTest.java | 1 - .../AlphaFilteredYoFrameVector3DTest.java | 1 - .../AlphaBetaFilteredYoVariableTest.java | 70 ++++ .../ButterworthFilteredYoVariableTest.java | 305 ++++++++++++++++++ .../SecondOrderFilteredYoDoubleTest.java | 6 +- 20 files changed, 708 insertions(+), 98 deletions(-) rename {ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math => ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables}/filters/AlphaBetaFilteredYoVariable.java (54%) create mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java delete mode 100644 ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilterType.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariableTest.java create mode 100644 ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariableTest.java diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java index 57f6d78c..51ae84ab 100644 --- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java @@ -5,7 +5,6 @@ import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D; import us.ihmc.yoVariables.filters.ProcessingYoVariable; -import us.ihmc.yoVariables.filters.SecondOrderFilterType; import us.ihmc.yoVariables.filters.SecondOrderFilteredYoDouble; import us.ihmc.yoVariables.filters.SecondOrderFilteredYoVariableParameters; import us.ihmc.yoVariables.registry.YoRegistry; @@ -46,7 +45,7 @@ public static SecondOrderFilteredYoFrameVector3D createSecondOrderFilteredYoFram double dt, double naturalFrequencyInHz, double dampingRatio, - SecondOrderFilterType filterType, + SecondOrderFilteredYoDouble.SecondOrderFilterType filterType, ReferenceFrame referenceFrame) { @@ -78,7 +77,7 @@ public static SecondOrderFilteredYoFrameVector3D createSecondOrderFilteredYoFram double dt, double naturalFrequencyInHz, double dampingRatio, - SecondOrderFilterType filterType, + SecondOrderFilteredYoDouble.SecondOrderFilterType filterType, YoFrameVector3D unfilteredVector) { SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java index 243da69a..ee5817bc 100644 --- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java @@ -6,6 +6,9 @@ import us.ihmc.yoVariables.variable.YoBoolean; import us.ihmc.yoVariables.variable.YoDouble; +/** + * Creates a yo variable + */ public class AccelerationLimitedYoVariable extends YoDouble { private final double dt; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBetaFilteredYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java similarity index 54% rename from ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBetaFilteredYoVariable.java rename to ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java index 58a221b7..6329dbe1 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaBetaFilteredYoVariable.java +++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java @@ -1,39 +1,22 @@ -package us.ihmc.robotics.math.filters; +package us.ihmc.yoVariables.filters; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; /** - *

Title:

+ *

+ * An {@code AlphaBetaFilteredYoVariable} is a filtered version of {@link YoDouble}, and estimates the velocity of some value it is tracking. + * Either a {@link YoDouble} holding the position value is passed in to the + * constructor and {@link #update()} is called every tick, or {@link #update(double)} is + * called every tick. The {@code AlphaBetaFilteredYoVariable} updates its value + * with the current filtered velocity using + *

+ * xp = x + (dt) v // angular position prediction + * x+ = xp + alpha (xmeas - xp) // adjusted angular position estimate + * v+ = v + beta (xmeas - xp) // adjusted velocity estimate * - *

Description:

- * - *

Copyright: Copyright (c) 2007

- * - *

Company:

- * - * @author not attributable - * @version 1.0 - */ - -/** - * @author thutcheson - *

- *

- *

- *

- * An AlphaBetaFilteredYoVariable is a filtered version of an input YoVar. - * Either a YoVariable holding the unfiltered val is passed in to the - * constructor and update() is called every tick, or update(double) is - * called every tick. The AlphaBetaFilteredYoVariable updates it's val - * with the current filtered velocity using - * - * xp = x + (dt) v // angular position prediction - * x+ = xp + alpha (xmeas - xp) // adjusted angular position estimate - * v+ = v + beta (xmeas - xp) // adjusted velocity estimate - * - *

- * 
+ * 

+ * 
  *
  *         For complete reference see:
  *                http://www.mstarlabs.com/control/engspeed.html#Ref2
@@ -51,8 +34,13 @@ public class AlphaBetaFilteredYoVariable extends YoDouble
    private final YoDouble positionState;
    private final YoDouble xMeasuredVariable;
 
-   public AlphaBetaFilteredYoVariable(String name, YoRegistry registry, double alpha, double beta, YoDouble positionVariable,
-         YoDouble xMeasuredVariable, double DT)
+   public AlphaBetaFilteredYoVariable(String name,
+                                      YoRegistry registry,
+                                      double alpha,
+                                      double beta,
+                                      YoDouble positionVariable,
+                                      YoDouble xMeasuredVariable,
+                                      double DT)
    {
       super(name, registry);
       this.alpha = alpha;
@@ -64,19 +52,6 @@ public AlphaBetaFilteredYoVariable(String name, YoRegistry registry, double alph
       reset();
    }
 
-   public AlphaBetaFilteredYoVariable(String name, YoRegistry registry, YoDouble alphaVariable, YoDouble betaVariable,
-         YoDouble positionVariable, YoDouble xMeasuredVariable, double DT)
-   {
-      super(name, registry);
-      this.alphaVariable = alphaVariable;
-      this.betaVariable = betaVariable;
-      this.DT = DT;
-      this.positionState = positionVariable;
-      this.xMeasuredVariable = xMeasuredVariable;
-
-      reset();
-   }
-
    public void reset()
    {
    }
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java
index a1c446b9..6f09dbe3 100644
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java
+++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java
@@ -7,7 +7,13 @@ public class AlphaFilterTools
 {
    /**
     * This computes the alpha
-    * variable for an alpha filtered yo variable that is equivalent to a first order low pass frequnecy at filter {@param breakFrequencyInHertz}
+    * variable for an alpha filtered yo variable that is equivalent to a first order low pass frequnecy at filter {@param breakFrequencyInHertz}.
+    *
+    * 

+    * If this
+    * calculation is done as part of a double provider, consider using {@link AlphaBasedOnBreakFrequencyProvider}, which only updates the alpha estimate
+    * on a change in the break frequency, saving computation.
+    * 

     *
     * @return alpha value
     */
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java
index 46e63224..47580e57 100644
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java
+++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java
@@ -14,31 +14,11 @@ public class AlphaFilteredYoMatrix extends YoMatrix
 
    private final YoDouble alpha;
 
-   public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, YoRegistry registry)
-   {
-      this(name, null, alpha, numberOfRows, numberOfColumns, null, null, registry);
-   }
-
-   public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, YoRegistry registry)
-   {
-      this(name, null, alpha, numberOfRows, numberOfColumns, rowNames, null, registry);
-   }
-
    public AlphaFilteredYoMatrix(String name, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry)
    {
       this(name, null, alpha, numberOfRows, numberOfColumns, rowNames, columnNames, registry);
    }
 
-   public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, YoRegistry registry)
-   {
-      this(name, description, alpha, numberOfRows, numberOfColumns, null, null, registry);
-   }
-
-   public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, YoRegistry registry)
-   {
-      this(name, description, alpha, numberOfRows, numberOfColumns, rowNames, null, registry);
-   }
-
    public AlphaFilteredYoMatrix(String name, String description, double alpha, int numberOfRows, int numberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry)
    {
       super(name, description, numberOfRows, numberOfColumns, rowNames, columnNames, registry);
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java
new file mode 100644
index 00000000..7622c3ce
--- /dev/null
+++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java
@@ -0,0 +1,152 @@
+package us.ihmc.yoVariables.filters;
+
+import us.ihmc.commons.MathTools;
+import us.ihmc.yoVariables.filters.AlphaFilteredYoVariable;
+import us.ihmc.yoVariables.providers.DoubleProvider;
+import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoBoolean;
+import us.ihmc.yoVariables.variable.YoDouble;
+
+/**
+ * @author jrebula
+ *         

+ *         

+ *         LittleDogVersion06: us.ihmc.LearningLocomotion.Version06.util.YoAlphaFilteredVariable,
+ *         9:34:00 AM, Aug 29, 2006
+ *         

+ *         

+ *         A YoAlphaFilteredVariable is a filtered version of an input YoVar. Either a YoVariable
+ *         holding the unfiltered val is passed in to the constructor and update() is called every
+ *         tick, or update(double) is called every tick. The YoAlphaFilteredVariable updates it's
+ *         val with the current filtered version using
+ *         

+ * 
+ *         
+ *            filtered_{n} = alpha * filtered_{n-1} + 1/2 * (1 - alpha) * (raw_{n} + raw{n-1}}
+ *         

+ */
+public class ButterworthFilteredYoVariable extends YoDouble
+{
+   public enum ButterworthFilterType
+   {
+      LOW_PASS, HIGH_PASS
+   }
+
+   private final DoubleProvider alphaVariable;
+   private final ButterworthFilterType butterworthFilterType;
+
+   private final DoubleProvider position;
+   private final YoDouble previousInput;
+
+   private final YoBoolean hasBeenCalled;
+
+   public ButterworthFilteredYoVariable(String name, YoRegistry registry, double alpha, ButterworthFilterType butterworthFilterType)
+   {
+      this(name, registry, AlphaFilteredYoVariable.createAlphaYoDouble(name, alpha, registry), butterworthFilterType);
+   }
+
+   public ButterworthFilteredYoVariable(String name,
+                                        YoRegistry registry,
+                                        double alpha,
+                                        DoubleProvider positionVariable,
+                                        ButterworthFilterType butterworthFilterType)
+   {
+      this(name, registry, AlphaFilteredYoVariable.createAlphaYoDouble(name, alpha, registry), positionVariable, butterworthFilterType);
+   }
+
+   public ButterworthFilteredYoVariable(String name, YoRegistry registry, DoubleProvider alphaVariable, ButterworthFilterType butterworthFilterType)
+   {
+      this(name, registry, alphaVariable, null, butterworthFilterType);
+   }
+
+   public ButterworthFilteredYoVariable(String name,
+                                        YoRegistry registry,
+                                        DoubleProvider alphaVariable,
+                                        DoubleProvider positionVariable,
+                                        ButterworthFilterType butterworthFilterType)
+   {
+      super(name, registry);
+
+      this.alphaVariable = alphaVariable;
+      this.butterworthFilterType = butterworthFilterType;
+
+      this.position = positionVariable;
+      this.previousInput = new YoDouble(name + "_prevIn", registry);
+      this.hasBeenCalled = new YoBoolean(name + "HasBeenCalled", registry);
+
+      reset();
+   }
+
+   public void reset()
+   {
+      hasBeenCalled.set(false);
+   }
+
+   public void update()
+   {
+      if (position == null)
+      {
+         throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null "
+               + "position variable to call update(), otherwise use update(double)");
+      }
+
+      update(position.getValue());
+   }
+
+   public void update(double currentInput)
+   {
+      if (!hasBeenCalled.getValue())
+      {
+         hasBeenCalled.set(true);
+
+         if (this.butterworthFilterType == ButterworthFilterType.HIGH_PASS)
+         {
+            set(0.0);
+         }
+         else
+         {
+            set(currentInput);
+         }
+      }
+      else
+      {
+         double alpha = alphaVariable.getValue();
+
+         switch (butterworthFilterType)
+         {
+            case LOW_PASS:
+            {
+               set(alpha * getValue() + 0.5 * (1.0 - alpha) * (currentInput + previousInput.getValue()));
+               break;
+            }
+            case HIGH_PASS:
+            {
+               set(alpha * getValue() + 0.5 * (1.0 + alpha) * (currentInput - previousInput.getValue()));
+               break;
+            }
+         }
+      }
+
+      previousInput.set(currentInput);
+   }
+
+   public static double computeAlphaGivenBreakFrequency(double breakFrequencyInHertz, double dt)
+   {
+      if (Double.isInfinite(breakFrequencyInHertz))
+         return 0.0;
+
+      double samplingFrequency = 1.0 / dt;
+
+      if (breakFrequencyInHertz > 0.25 * samplingFrequency)
+         return 0.0;
+
+      double tanOmegaBreak = Math.tan(Math.PI * breakFrequencyInHertz * dt);
+      return MathTools.clamp((1.0 - tanOmegaBreak) / (1.0 + tanOmegaBreak), 0.0, 1.0);
+   }
+
+   public static double computeBreakFrequencyGivenAlpha(double alpha, double dt)
+   {
+      double beta = (1.0 - alpha) / (1.0 + alpha);
+      return Math.max(Math.atan(beta) / (dt * Math.PI), 0.0);
+   }
+}
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java
index 23c41014..e548ae57 100644
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java
+++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java
@@ -37,11 +37,6 @@ public class FilteredDiscreteVelocityYoVariable extends YoDouble
    private final YoDouble lastPosition;
    private boolean hasBeenCalled;
 
-   public FilteredDiscreteVelocityYoVariable(String name, String description, double alpha, YoDouble time, YoRegistry registry)
-   {
-      this(name, description, alpha, null, time, registry);
-   }
-
    public FilteredDiscreteVelocityYoVariable(String name, String description, double alpha, YoDouble positionVariable, YoDouble time,
          YoRegistry registry)
    {
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
index 08596c2b..626d02d0 100644
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
+++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
@@ -10,7 +10,7 @@ public class FirstOrderBandPassFilteredYoDouble extends YoDouble
    private final YoBoolean hasBeenCalled;
 
    private double filterUpdateTimeOld;
-   private FirstOrderFilterType filterType;
+   private final FirstOrderFilterType filterType;
 
    private final YoDouble firstCutoffFrequencyHz;
    private final YoDouble secondCutoffFrequencyHz;
@@ -18,10 +18,9 @@ public class FirstOrderBandPassFilteredYoDouble extends YoDouble
    private final DoubleProvider yoTime;
    private double dt;
 
-
    public enum FirstOrderFilterType
    {
-       NOTCH, BAND
+      NOTCH, BAND
    }
 
    public FirstOrderBandPassFilteredYoDouble(String name,
@@ -176,7 +175,7 @@ private double applyLowPassFilter(double filterInput, double breakFrequency, dou
       return alpha * this.getDoubleValue() + (1.0 - alpha) * filterInput;
    }
 
-   private double applyHighPassFilter(double filterInput,  double breakFrequency, double dt)
+   private double applyHighPassFilter(double filterInput, double breakFrequency, double dt)
    {
       double lowPassValue = applyLowPassFilter(filterInput, breakFrequency, dt);
 
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
index ef26ebab..89db7267 100644
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
+++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
@@ -20,7 +20,7 @@ public enum FirstOrderFilterType
    private final DoubleProvider yoTime;
    private double dt;
 
-   private FirstOrderFilterType filterType;
+   private final FirstOrderFilterType filterType;
 
    public FirstOrderFilteredYoDouble(String name,
                                      String description,
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java
index d3182ab8..b38e0214 100644
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java
+++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java
@@ -2,9 +2,9 @@
 
 public interface ProcessingYoVariable
 {
-   public abstract void update();
+   void update();
 
-   public default void reset()
+   default void reset()
    {
    }
 }
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java
index bcc4b17f..475a49ab 100644
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java
+++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java
@@ -5,6 +5,11 @@
 import us.ihmc.yoVariables.variable.YoBoolean;
 import us.ihmc.yoVariables.variable.YoDouble;
 
+/**
+ * This is a yo variable whose rate of change is clamped to some maximum value. To use it, either create this variable passing in the non-limited yo variable to
+ * track and then call {@link #update()}, or unlink it keep it unlinked from a specific yo variable and call {@link #update(double)}. This variable when then
+ * track the specified variable or input, but will not change faster than the rate provided by {@code maxRateVariable}.
+ */
 public class RateLimitedYoVariable extends YoDouble
 {
    private final DoubleProvider maxRateVariable;
@@ -16,21 +21,111 @@ public class RateLimitedYoVariable extends YoDouble
 
    private final YoBoolean hasBeenCalled;
 
+   /**
+    * Constructs this variable with no double to track. The value contained in this yo variable will track the desired reference, but will limit the maximum
+    * rate of change to {@code maxRate}.
+    * 

+    * The maximum rate of change is enforced as a maximum step size every time {@link #update(double)} is called. The maximum step size
+    * can be calculated as {@code maxRate} * {@code dt}.
+    * 

+    * 

+    * To use this variable after using this constructor, you must call {@link #update(double)}.  Calling {@link #update()} will result in a null pointer
+    * exception.
+    * 

+    * 

+    * A known edge case is if {@link #update(double)} is called more than once per control update. In this case, the maximum rate is enforced each time
+    * {@link #update()}  called, rather than per each control update. Avoid doing this.
+    * 

+    *
+    * @param name     name of this variable.
+    * @param registry registry to add this variable to.
+    * @param maxRate  maximum rate of change this value can experience every time {@link #update(double)} is called.
+    * @param dt       expected time change since between calls of {@link #update(double)}.
+    */
    public RateLimitedYoVariable(String name, YoRegistry registry, double maxRate, double dt)
    {
       this(name, registry, maxRate, null, dt);
    }
 
+   /**
+    * Constructs this variable with no double to track. The value contained in this yo variable will track the desired reference, but will limit the maximum
+    * rate of change to {@code maxRateVariable}.
+    * 

+    * The maximum rate of change is enforced as a maximum step size every time  {@link #update(double)} is called. The maximum step size
+    * can be calculated as {@code maxRateVariable} * {@code dt}.
+    * 

+    * 

+    * To use this variable after using this constructor, you must call {@link #update(double)}.  Calling {@link #update()} will result in a null pointer
+    * exception.
+    * 

+    * 

+    * A known edge case is if {@link #update(double)} is called more than once per control update. In this case, the maximum rate is enforced each time
+    * {@link #update()}  called, rather than per each control update. Avoid doing this.
+    * 

+    *
+    * @param name            name of this variable.
+    * @param registry        registry to add this variable to.
+    * @param maxRateVariable maximum rate of change this value can experience every time {@link #update(double)} is called.
+    * @param dt              expected time change since between calls of {@link #update(double)}.
+    */
    public RateLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRateVariable, double dt)
    {
       this(name, registry, maxRateVariable, null, dt);
    }
 
+   /**
+    * Constructs this variable to track {@code positionVariable}. The value contained in this yo variable will track the desired reference passed in at
+    * construction, but will limit the maximum
+    * rate of change to {@code maxRate}.
+    * 

+    * The maximum rate of change is enforced as a maximum step size every time {@link #update()} is called. The maximum step size
+    * can be calculated as {@code maxRate} * {@code dt}.
+    * 

+    * 

+    * To use this variable after using this constructor, you should call {@link #update()}.  Calling {@link #update(double)} will not track the variable
+    * provided
+    * by {@code positionVariable}.
+    * 

+    * 

+    * A known edge case is if {@link #update()} is called more than once per control update. In this case, the maximum rate is enforced each time
+    * {@link #update()}  called, rather than per each control update. Avoid doing this.
+    * 

+    *
+    * @param name             name of this variable.
+    * @param registry         registry to add this variable to.
+    * @param maxRate          maximum rate of change this value can experience every time {@link #update()} is called.
+    * @param positionVariable varible provider that this variable will track.
+    * @param dt               expected time change since between calls of {@link #update()}.
+    */
    public RateLimitedYoVariable(String name, YoRegistry registry, double maxRate, DoubleProvider positionVariable, double dt)
    {
       this(name, registry, VariableTools.createMaxRateYoDouble(name, "", maxRate, registry), positionVariable, dt);
    }
 
+   /**
+    * Constructs this variable to track {@code positionVariable}. The value contained in this yo variable will track the desired reference passed in at
+    * construction, but will limit the maximum
+    * rate of change to {@code maxRateVariable}.
+    * 

+    * The maximum rate of change is enforced as a maximum step size every time {@link #update()} is called. The maximum step size
+    * can be calculated as {@code maxRateVariable} * {@code dt}.
+    * 

+    * 

+    * To use this variable after using this constructor, you should call {@link #update()}.  Calling {@link #update(double)} will not track the variable
+    * provided
+    * by {@code positionVariable}.
+    * 

+    * 

+    * A known edge case is if {@link #update()} is called more than once per control update. In this case, the maximum rate is enforced each time
+    * {@link #update()}  called, rather than per each control update. Avoid doing this.
+    * 

+    *
+    * @param name              name of this variable.
+    * @param registry          registry to add this variable to.
+    * @param maxRateVariable   maximum rate of change this value can experience every time {@link #update()} is called.
+    * @param unlimitedPosition varible provider that this variable will track.
+    * @param dt                expected time change since between calls of {@link #update()}.
+    */
    public RateLimitedYoVariable(String name, YoRegistry registry, DoubleProvider maxRateVariable, DoubleProvider unlimitedPosition, double dt)
    {
       super(name, registry);
@@ -46,22 +141,57 @@ public RateLimitedYoVariable(String name, YoRegistry registry, DoubleProvider ma
       reset();
    }
 
+   /**
+    * Resets this variable. On the next time {@link #update()} or {@link #update(double)} is called, it will automatically be set to the variable to track,
+    * rather than experiencing any rate limiting.
+    */
    public void reset()
    {
       hasBeenCalled.set(false);
    }
 
+   /**
+    * Updates the value contained in this yo variable to track the value contained in {@code unlimitedPosition}. If the {@code unlimitedPosition} can be
+    * achieved with a rate of change
+    * less that provided by {@code maxRateVariable}, then the value stored in this variable and returned by {@link #getValue()} will match
+    * {@code unlimitedPosition}.
+    * Otherwise, it will step towards the new position at maximum rate. This can be computed using the following pseudo-code:
+    * 

+    * 
  • error = currentPosition - getValue()
    • 
+    * 
  • if abs(error) < maxRate * dt
    • 
+    * 
  •   set(currentPosition)
    • 
+    * 
  • else
    • 
+    * 
  •   set(currentPosition + sign(error) * maxRate * dt)
    • 
+    * 
    
+    *
+    * @throws NullPointerException if this class was constructed with no {@code unlimitedPosition} variable to track.
+    */
    public void update()
    {
       if (unlimitedPosition == null)
       {
-         throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null "
-               + "position variable to call update(), otherwise use update(double)");
+         throw new NullPointerException(
+               getClass().getSimpleName() + " must be constructed with a non null " + "position variable to call update(), otherwise use update(double)");
       }
 
       update(unlimitedPosition.getValue());
    }
 
+   /**
+    * Updates the value contained in this yo variable to track {@code currentPosition}. If the {@code currentPosition} can be achieved with a rate of change
+    * less that provided by {@code maxRateVariable}, then the value stored in this variable and returned by {@link #getValue()} will match
+    * {@code currentPosition}.
+    * Otherwise, it will step towards the new position at maximum rate. This can be computed using the following pseudo-code:
+    * 
    
+    * 
  • error = currentPosition - getValue()
    • 
+    * 
  • if abs(error) < maxRate * dt
    • 
+    * 
  •   set(currentPosition)
    • 
+    * 
  • else
    • 
+    * 
  •   set(currentPosition + sign(error) * maxRate * dt)
    • 
+    * 
    
+    *
+    * @param currentPosition position to try and achieve within rate limits.
+    */
    public void update(double currentPosition)
    {
       if (!hasBeenCalled.getBooleanValue())
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilterType.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilterType.java
deleted file mode 100644
index d524c37e..00000000
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilterType.java
+++ /dev/null
@@ -1,6 +0,0 @@
-package us.ihmc.yoVariables.filters;
-
-public enum SecondOrderFilterType
-{
-   LOW_PASS, NOTCH, BAND, HIGH_PASS
-}
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java
index 28893835..89cda9cb 100644
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java
+++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java
@@ -201,4 +201,9 @@ private void computeCoefficients()
       a[1] = 2.0 * omega * omega - 8.0 / (dt * dt);
       a[2] = 4.0 / (dt * dt) - 4.0 / dt * xi * omega + omega * omega;
    }
+
+   public enum SecondOrderFilterType
+   {
+      LOW_PASS, NOTCH, BAND, HIGH_PASS
+   }
 }
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java
index 68bf4f78..b7ce88ad 100644
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java
+++ b/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java
@@ -7,10 +7,10 @@ public class SecondOrderFilteredYoVariableParameters
 {
    private final YoDouble naturalFrequencyInHz;
    private final YoDouble dampingRatio;
-   private final SecondOrderFilterType filterType;
+   private final SecondOrderFilteredYoDouble.SecondOrderFilterType filterType;
 
    public SecondOrderFilteredYoVariableParameters(String name, YoRegistry registry, double naturalFrequencyInHz, double dampingRatio,
-         SecondOrderFilterType filterType)
+         SecondOrderFilteredYoDouble.SecondOrderFilterType filterType)
    {
       this.naturalFrequencyInHz = new YoDouble(name + "NaturalFrequency", registry);
       this.naturalFrequencyInHz.set(naturalFrequencyInHz);
@@ -29,7 +29,7 @@ public YoDouble getDampingRatio()
       return dampingRatio;
    }
 
-   public SecondOrderFilterType getFilterType()
+   public SecondOrderFilteredYoDouble.SecondOrderFilterType getFilterType()
    {
       return filterType;
    }
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java
index 103c0504..abbe1fa5 100644
--- a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java
+++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java
@@ -7,7 +7,6 @@
 
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools;
-import us.ihmc.euclid.tools.EuclidCoreMissingRandomTools;
 import us.ihmc.euclid.tools.EuclidCoreRandomTools;
 import us.ihmc.euclid.tools.EuclidCoreTestTools;
 import us.ihmc.euclid.tuple3D.Point3D;
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java
index 7962aac5..009a3b90 100644
--- a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java
+++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java
@@ -12,7 +12,6 @@
 import us.ihmc.euclid.tools.EuclidCoreRandomTools;
 import us.ihmc.euclid.tools.EuclidCoreTestTools;
 import us.ihmc.euclid.tuple4D.Quaternion;
-import us.ihmc.euclid.tools.EuclidCoreMissingRandomTools;
 import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameQuaternion;
 import us.ihmc.yoVariables.providers.DoubleProvider;
 import us.ihmc.yoVariables.registry.YoRegistry;
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java
index a2be79c3..3d4a6c3f 100644
--- a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java
+++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java
@@ -7,7 +7,6 @@
 
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools;
-import us.ihmc.euclid.tools.EuclidCoreMissingRandomTools;
 import us.ihmc.euclid.tools.EuclidCoreRandomTools;
 import us.ihmc.euclid.tools.EuclidCoreTestTools;
 import us.ihmc.euclid.tuple3D.Point3D;
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariableTest.java
new file mode 100644
index 00000000..01462b6d
--- /dev/null
+++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariableTest.java
@@ -0,0 +1,70 @@
+package us.ihmc.yoVariables.filters;
+
+import org.junit.jupiter.api.Test;
+
+import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoDouble;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+public class AlphaBetaFilteredYoVariableTest
+{
+   private static final double DT = 0.1;
+
+	@Test
+   public void testAlphaBetaFilteredVelocityAndPositionEstimatesWithNoVelocity()
+   {
+      YoRegistry registry = new YoRegistry("testRegistry");
+      YoDouble positionVariable = new YoDouble("positionVariable", registry);
+      YoDouble xMeasuredVariable = new YoDouble("xMeasuredVariable", registry);
+
+      double alpha = 0.2;
+      double beta = 0.35;
+
+      AlphaBetaFilteredYoVariable abFilteredYoVariable = new AlphaBetaFilteredYoVariable("abFilteredYoVariable", registry, alpha, beta, positionVariable,
+                                                                                         xMeasuredVariable, DT);
+
+      abFilteredYoVariable.set(0);
+      positionVariable.set(0);
+      xMeasuredVariable.set(42);
+
+      for (int i = 0; i < 10000; i++)
+      {
+         abFilteredYoVariable.update();
+      }
+
+      // Converges on 0 since position doesn't change (no dx/dt)
+      assertEquals(0, abFilteredYoVariable.getDoubleValue(), 1e-7);
+
+   }
+
+	@Test
+   public void testAlphaBetaFilteredVelocityAndPositionEstimatesWithConstantVelocity()
+   {
+      YoRegistry registry = new YoRegistry("testRegistry");
+      YoDouble positionVariable = new YoDouble("positionVariable", registry);
+      YoDouble xMeasuredVariable = new YoDouble("xMeasuredVariable", registry);
+
+      double alpha = 0.2;
+      double beta = 0.35;
+
+      AlphaBetaFilteredYoVariable abFilteredYoVariable = new AlphaBetaFilteredYoVariable("abFilteredYoVariable", registry, alpha, beta, positionVariable,
+            xMeasuredVariable, DT);
+
+      for (int i = 0; i < 10000; i++)
+      {
+         abFilteredYoVariable.set(0);
+         positionVariable.set(0);
+         xMeasuredVariable.set(42);
+
+         for (int j = 0; j < 10000; j++)
+         {
+            xMeasuredVariable.set(xMeasuredVariable.getDoubleValue() + 10); // Velocity = 100 distances per time
+            abFilteredYoVariable.update();
+         }
+
+         assertEquals(100, abFilteredYoVariable.getDoubleValue(), 1e-7);
+      }
+   }
+
+}
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariableTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariableTest.java
new file mode 100644
index 00000000..accd30ac
--- /dev/null
+++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariableTest.java
@@ -0,0 +1,305 @@
+package us.ihmc.yoVariables.filters;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import java.util.Random;
+
+import org.junit.jupiter.api.Disabled;
+import org.junit.jupiter.api.Test;
+
+import us.ihmc.commons.RandomNumbers;
+import us.ihmc.yoVariables.filters.ButterworthFilteredYoVariable.ButterworthFilterType;
+import us.ihmc.yoVariables.registry.YoRegistry;
+
+public class ButterworthFilteredYoVariableTest
+{
+   @Test
+   public void testAlphaCompute()
+   {
+      Random random = new Random(0734454);
+      double epsilon = 1.0e-12;
+
+      for (int i = 0; i < 5000; i++)
+      {
+         double dt = RandomNumbers.nextDouble(random, 1.0e-4, 1.0e-2);
+         double samplingFrequency = 1.0 / dt;
+
+         double breakFrequencyIn, alpha, breakFrequencyOut;
+
+         breakFrequencyIn = 0.0;
+         alpha = ButterworthFilteredYoVariable.computeAlphaGivenBreakFrequency(breakFrequencyIn, dt);
+         assertEquals(1.0, alpha, epsilon);
+         breakFrequencyOut = ButterworthFilteredYoVariable.computeBreakFrequencyGivenAlpha(alpha, dt);
+         assertEquals(breakFrequencyIn, breakFrequencyOut, epsilon);
+
+         breakFrequencyIn = RandomNumbers.nextDouble(random, 0.0, 0.25 * samplingFrequency);
+         alpha = ButterworthFilteredYoVariable.computeAlphaGivenBreakFrequency(breakFrequencyIn, dt);
+         breakFrequencyOut = ButterworthFilteredYoVariable.computeBreakFrequencyGivenAlpha(alpha, dt);
+         assertEquals(breakFrequencyIn, breakFrequencyOut, epsilon);
+
+         breakFrequencyIn = RandomNumbers.nextDouble(random, 0.25 * samplingFrequency, 10.0 * samplingFrequency);
+         alpha = ButterworthFilteredYoVariable.computeAlphaGivenBreakFrequency(breakFrequencyIn, dt);
+         assertEquals(0.0, alpha, epsilon);
+         breakFrequencyOut = ButterworthFilteredYoVariable.computeBreakFrequencyGivenAlpha(alpha, dt);
+         assertEquals(0.25 * samplingFrequency, breakFrequencyOut, epsilon);
+      }
+   }
+
+   @Test
+   public void testBreakFrequencyLowPassFilter()
+   {
+      double dt = 0.001;
+      double desiredBreakFrequency = 4.0;
+      double alpha = ButterworthFilteredYoVariable.computeAlphaGivenBreakFrequency(desiredBreakFrequency, dt);
+      ButterworthFilteredYoVariable butterworthFilteredYoVariable = new ButterworthFilteredYoVariable("test", null, alpha, ButterworthFilterType.LOW_PASS);
+
+      double actualBreakFrequency = findBreakFrequency(butterworthFilteredYoVariable, dt, dt);
+      double percentError = Math.abs(actualBreakFrequency - desiredBreakFrequency) / desiredBreakFrequency;
+      // TODO After a bunch of reading, I'm pretty sure the break frequency is properly calculated.
+      // The algorithm for finding the break frequency seems to be doing the right thing.
+      // We get a percent error of 147% for a break frequency of 100Hz, so I'm missing something somewhere...
+      assertEquals(0.0, percentError, 0.05);
+
+      //      Random random = new Random(45353);
+      //      int numberOfCycles = 20;
+      //
+      //      for (int i = 0; i < 1000; i++)
+      //      {
+      //         dt = 0.001;
+      //         double samplingFrequency = 1.0 / dt;
+      //         desiredBreakFrequency = RandomNumbers.nextDouble(random, 0.0, 0.25 * samplingFrequency);
+      //         alpha = ButterworthFilteredYoVariable.computeAlphaGivenBreakFrequency(desiredBreakFrequency, dt);
+      //
+      //         butterworthFilteredYoVariable = new ButterworthFilteredYoVariable("test", null, alpha, ButterworthFilterType.LOW_PASS);
+      //
+      //
+      //         double testBreakFrequency = desiredBreakFrequency;
+      //         int length = (int) Math.max(Math.ceil(numberOfCycles / (testBreakFrequency * dt)) + 1, 1000);
+      //
+      //         TimedData[] inputCurve = generateInputCurve(length, testBreakFrequency, dt);
+      //         TimedData[] outputCurve = getFilteredCurve(inputCurve, butterworthFilteredYoVariable);
+      //
+      //         inputCurve = Arrays.copyOfRange(inputCurve, inputCurve.length / 2, inputCurve.length);
+      //         outputCurve = Arrays.copyOfRange(outputCurve, outputCurve.length / 2, outputCurve.length);
+      //
+      //         double actualAttenuation = getMagnitudeInDecibels(inputCurve, outputCurve);
+      //         double expectedAttenuation = -Math.abs(computePredictedAttenuationInDecibels(desiredBreakFrequency, 1, testBreakFrequency));
+      //
+      //         assertEquals(expectedAttenuation, actualAttenuation, 10.0 * dt, "Iteration: " + i + ", desiredBreakFrequency: " + desiredBreakFrequency);
+      //      }
+   }
+
+   public static double computePredictedAttenuationInDecibels(double cutOffFrequency, int filterOrder, double queryFrequency)
+   {
+      return 10.0 * Math.log10(1.0 + Math.pow(queryFrequency / cutOffFrequency, 2.0 * filterOrder));
+   }
+
+   @Disabled // Old code
+   @Test
+   public void testButterWorth()
+   {
+      YoRegistry registry = new YoRegistry("Test");
+      double dt = 0.001;
+      double desiredBreakFrequency = 1.0;
+      double alpha = ButterworthFilteredYoVariable.computeAlphaGivenBreakFrequency(desiredBreakFrequency, dt);
+      double testBreakFrequency = desiredBreakFrequency;
+      System.out.println(ButterworthFilteredYoVariable.computeAlphaGivenBreakFrequency(testBreakFrequency, dt));
+
+      ButterworthFilteredYoVariable butterworthFilteredYoVariable = new ButterworthFilteredYoVariable("test", registry, alpha, ButterworthFilterType.LOW_PASS);
+
+      int numberOfCycles = 6;
+      double endTime = (numberOfCycles) / testBreakFrequency;
+
+      TimedData[] inputCurve = generateInputCurve(endTime, testBreakFrequency, dt);
+      TimedData[] outputCurve = getFilteredCurve(inputCurve, butterworthFilteredYoVariable);
+
+      TimedData[] clippedInputCurve = Arrays.copyOfRange(inputCurve, inputCurve.length / 2, inputCurve.length);
+      TimedData[] clippedOutputCurve = Arrays.copyOfRange(outputCurve, outputCurve.length / 2, outputCurve.length);
+
+      double dB = getMagnitudeInDecibels(clippedInputCurve, clippedOutputCurve);
+      System.out.println("dB= " + dB);
+      System.out.println(computePredictedAttenuationInDecibels(desiredBreakFrequency, 1, testBreakFrequency));
+   }
+
+   private static TimedData[] generateInputCurve(double endTime, double frequency, double dt)
+   {
+      return generateInputCurve((int) Math.ceil(endTime / dt) + 1, frequency, dt);
+   }
+
+   private static TimedData[] generateInputCurve(int numberOfElements, double frequency, double dt)
+   {
+      TimedData[] inputCurve = new TimedData[numberOfElements];
+
+      double time = 0.0;
+
+      for (int i = 0; i < numberOfElements; i++)
+      {
+         inputCurve[i] = new TimedData(time, Math.sin(2.0 * Math.PI * frequency * time));
+         time += dt;
+      }
+
+      return inputCurve;
+   }
+
+   private static double findBreakFrequency(ButterworthFilteredYoVariable filter, double dt, double tolerance)
+   {
+      int numberOfCycles = 10;
+
+      double samplingFrequency = 1.0 / dt;
+      double upperBreakFrequency = 0.5 * samplingFrequency;
+      double lowerBreakFrequency = 0.0;
+      double upper_dB = Double.NEGATIVE_INFINITY;
+      double lower_dB = 0.0;
+
+      double cutOff_dB = -computePredictedAttenuationInDecibels(samplingFrequency, 1, samplingFrequency);
+
+      while (Math.abs(upper_dB - lower_dB) > tolerance && Math.abs(upperBreakFrequency - lowerBreakFrequency) > 0.01 * dt)
+      {
+         double testBreakFrequency = 0.5 * (upperBreakFrequency + lowerBreakFrequency);
+         double endTime = (numberOfCycles) / testBreakFrequency;
+         TimedData[] inputCurve = generateInputCurve(endTime, testBreakFrequency, dt);
+         TimedData[] outputCurve = getFilteredCurve(inputCurve, filter);
+
+         TimedData[] clippedInputCurve = Arrays.copyOfRange(inputCurve, inputCurve.length / 2, inputCurve.length);
+         TimedData[] clippedOutputCurve = Arrays.copyOfRange(outputCurve, outputCurve.length / 2, outputCurve.length);
+
+         double dB = getMagnitudeInDecibels(clippedInputCurve, clippedOutputCurve);
+         System.out.println("Test breakFrequency: " + testBreakFrequency + ", dB: " + dB);
+
+         if (dB < cutOff_dB)
+         {
+            upperBreakFrequency = testBreakFrequency;
+            upper_dB = dB;
+         }
+         else
+         {
+            lowerBreakFrequency = testBreakFrequency;
+            lower_dB = dB;
+         }
+      }
+
+      return 0.5 * (upperBreakFrequency + lowerBreakFrequency);
+   }
+
+   private static TimedData[] getFilteredCurve(TimedData[] input, ButterworthFilteredYoVariable butterworthFilteredYoVariable)
+   {
+      TimedData[] filteredCurve = new TimedData[input.length];
+
+      butterworthFilteredYoVariable.reset();
+
+      for (int i = 0; i < input.length; i++)
+      {
+         butterworthFilteredYoVariable.update(input[i].value);
+
+         filteredCurve[i] = new TimedData(input[i].time, butterworthFilteredYoVariable.getDoubleValue());
+      }
+
+      return filteredCurve;
+   }
+
+   private static double plotBodeForAlpha(double alpha, double dt)
+   {
+      YoRegistry registry = new YoRegistry("Test");
+
+      ButterworthFilteredYoVariable butterworthFilteredYoVariable = new ButterworthFilteredYoVariable("test", registry, alpha, ButterworthFilterType.LOW_PASS);
+
+      double startFreq = 1e-2;
+      double endFreq = 0.25 * (1.0 / dt);
+
+      int numberOfTestPoints = 100;
+
+      double deltaFreq = (endFreq - startFreq) / (numberOfTestPoints - 1);
+
+      List testFrequencies = new ArrayList<>();
+      List attenuations = new ArrayList<>();
+
+      for (double freq = startFreq; freq <= endFreq; freq = freq + deltaFreq)
+      {
+         int numberOfCycles = 3;
+         double endTime = (numberOfCycles) / freq;
+
+         TimedData[] inputCurve = generateInputCurve(endTime, freq, dt);
+         TimedData[] outputCurve = getFilteredCurve(inputCurve, butterworthFilteredYoVariable);
+
+         double dB = getMagnitudeInDecibels(inputCurve, outputCurve);
+
+         testFrequencies.add(freq);
+         attenuations.add(dB);
+      }
+
+      @SuppressWarnings("unused")
+      List testFrequenciesLog = convertToLog(testFrequencies);
+
+      double breakFreq = getBreakFreq(testFrequencies, attenuations);
+
+      return breakFreq;
+   }
+
+   private static double getBreakFreq(List freq, List attenutation_dB)
+   {
+      for (int i = 0; i < freq.size(); i++)
+      {
+         double attenuation = attenutation_dB.get(i);
+
+         if (attenuation < -3.0)
+            return freq.get(i);
+      }
+
+      return Double.POSITIVE_INFINITY;
+   }
+
+   private static ArrayList convertToLog(List arrayListToConvert)
+   {
+      ArrayList ret = new ArrayList<>();
+
+      for (Double value : arrayListToConvert)
+      {
+         double logValue = Math.log10(value);
+         ret.add(logValue);
+      }
+
+      return ret;
+   }
+
+   private static double getMagnitudeInDecibels(TimedData[] input, TimedData[] output)
+   {
+      double inputAmp = getMaximumPeakToPeakAmplitude(input);
+      double outputAmp = getMaximumPeakToPeakAmplitude(output);
+      double attenuation = outputAmp / inputAmp;
+      return 10.0 * Math.log10(attenuation);
+   }
+
+   private static double getMaximumPeakToPeakAmplitude(TimedData[] dataset)
+   {
+      double value = dataset[0].value;
+      double maximumValue = value;
+      double minimumValue = value;
+
+      for (int i = 1; i < dataset.length; i++)
+      {
+         value = dataset[1].value;
+
+         if (value > maximumValue)
+            maximumValue = value;
+         else if (value < minimumValue)
+            minimumValue = value;
+      }
+
+      return maximumValue - minimumValue;
+   }
+
+   private static class TimedData
+   {
+      private double time;
+      private double value;
+
+      public TimedData(double time, double value)
+      {
+         this.time = time;
+         this.value = value;
+      }
+   }
+}
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java
index 93acfeaa..887a68dc 100644
--- a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java
+++ b/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java
@@ -19,7 +19,7 @@ public void testLowPassFilterCoefficients()
       double[] aAssert = {4255275.254047619, -7992104.316479129, 3752620.429473252};
 
       SecondOrderFilteredYoDouble filteredYoVariable = new SecondOrderFilteredYoDouble("lowPass", registry, dt, naturalFrequencyInHz, dampingRatio,
-                                                                                       SecondOrderFilterType.LOW_PASS);
+                                                                                       SecondOrderFilteredYoDouble.SecondOrderFilterType.LOW_PASS);
 
       double[] b = new double[3];
       double[] a = new double[3];
@@ -38,7 +38,7 @@ public void testNotchFilterCoefficients()
       double[] aAssert = {4255275.254047619, -7992104.316479129, 3752620.429473252};
 
       SecondOrderFilteredYoDouble filteredYoVariable = new SecondOrderFilteredYoDouble("notch", registry, dt, naturalFrequencyInHz, dampingRatio,
-                                                                                       SecondOrderFilterType.NOTCH);
+                                                                                       SecondOrderFilteredYoDouble.SecondOrderFilterType.NOTCH);
 
       double[] b = new double[3];
       double[] a = new double[3];
@@ -57,7 +57,7 @@ public void testHighPassFilterCoefficients()
       double[] aAssert = {4255275.254047619, -7992104.316479129, 3752620.429473252};
 
       SecondOrderFilteredYoDouble filteredYoVariable = new SecondOrderFilteredYoDouble("highPass", registry, dt, naturalFrequencyInHz, dampingRatio,
-                                                                                       SecondOrderFilterType.HIGH_PASS);
+                                                                                       SecondOrderFilteredYoDouble.SecondOrderFilterType.HIGH_PASS);
 
       double[] b = new double[3];
       double[] a = new double[3];

From 5c9316de46b796859eeb05b69cc5751e6dca4545 Mon Sep 17 00:00:00 2001
From: "rgriffin@ihmc.org" 
Date: Tue, 22 Oct 2024 14:27:37 -0500
Subject: [PATCH 43/47] pulled in quite a few of the filtered yo variables

---
 build.gradle.kts                              |   2 +
 ihmc-yovariables-filters/LICENSE.txt          | 202 ------------------
 ihmc-yovariables-filters/build.gradle.kts     |  24 ---
 ihmc-yovariables-filters/gradle.properties    |   6 -
 ihmc-yovariables-filters/settings.gradle.kts  |  23 --
 .../AccelerationLimitedYoFrameVector3D.java   |   0
 .../AlphaFilteredRigidBodyTransform.java      |   0
 .../euclid/filters/AlphaFilteredTuple2D.java  |   0
 .../euclid/filters/AlphaFilteredTuple3D.java  |   0
 .../filters/AlphaFilteredYoFramePoint2D.java  |   0
 .../filters/AlphaFilteredYoFramePoint3D.java  |   0
 .../filters/AlphaFilteredYoFramePose3D.java   |   0
 .../AlphaFilteredYoFrameQuaternion.java       |   0
 .../filters/AlphaFilteredYoFrameVector2D.java |   0
 .../filters/AlphaFilteredYoFrameVector3D.java |   0
 .../AlphaFilteredYoMutableFrameVector3D.java  |   0
 ...shCompensatingVelocityYoFrameVector3D.java |   0
 .../BacklashProcessingYoFrameVector3D.java    |   0
 .../filters/DeadbandedYoFrameVector3D.java    |   0
 ...lteredFiniteDifferenceYoFrameVector2D.java |   0
 ...lteredFiniteDifferenceYoFrameVector3D.java |   0
 ...ferenceAngularVelocityYoFrameVector3D.java |   0
 .../filters/MovingAverageYoFramePoint2D.java  |   0
 .../filters/MovingAverageYoFrameVector2D.java |   0
 .../RateLimitedYoFrameOrientation.java        |   0
 .../filters/RateLimitedYoFramePoint2D.java    |   0
 .../filters/RateLimitedYoFramePoint3D.java    |   0
 .../filters/RateLimitedYoFramePose3D.java     |   0
 .../filters/RateLimitedYoFrameQuaternion.java |   0
 .../filters/RateLimitedYoFrameVector2D.java   |   0
 .../filters/RateLimitedYoFrameVector3D.java   |   0
 .../RateLimitedYoMutableFrameVector3D.java    |   0
 .../RunningAverageYoFrameVector3D.java        |   0
 .../SecondOrderFilteredYoFrameVector3D.java   |   0
 ...eMovingAverageFilteredYoFrameVector3D.java |   0
 .../AccelerationLimitedYoVariable.java        |   0
 .../AlphaBasedOnBreakFrequencyProvider.java   |   0
 .../filters/AlphaBetaFilteredYoVariable.java  |   0
 .../yoVariables/filters/AlphaFilterTools.java |   0
 .../AlphaFilteredWrappingYoVariable.java      |   0
 .../filters/AlphaFilteredYoMatrix.java        |   0
 .../filters/AlphaFilteredYoVariable.java      |   0
 ...acklashCompensatingVelocityYoVariable.java |   0
 .../filters/BacklashProcessingYoVariable.java |   0
 .../yoVariables/filters/BacklashState.java    |   0
 .../ButterworthFilteredYoVariable.java        |   0
 .../filters/DeadbandedYoVariable.java         |   0
 .../yoVariables/filters/DelayedYoBoolean.java |   0
 .../yoVariables/filters/DelayedYoDouble.java  |   0
 .../filters/DeltaLimitedYoVariable.java       |   0
 .../FilteredDiscreteVelocityYoVariable.java   |   0
 .../FilteredFiniteDifferenceYoVariable.java   |   4 +-
 .../FirstOrderBandPassFilteredYoDouble.java   |   0
 .../filters/FirstOrderFilteredYoDouble.java   |   0
 .../filters/GlitchFilteredYoBoolean.java      |   0
 .../filters/GlitchFilteredYoInteger.java      |   0
 .../filters/JerkLimitedYoDouble.java          |   0
 .../filters/MovingAverageYoDouble.java        |   0
 .../filters/ProcessingYoVariable.java         |   0
 .../filters/RateLimitedYoVariable.java        |   0
 .../filters/RunningAverageYoDouble.java       |   0
 .../filters/SecondOrderFilteredYoDouble.java  |   0
 ...condOrderFilteredYoVariableParameters.java |   0
 ...SimpleMovingAverageFilteredYoVariable.java |   5 +-
 .../yoVariables/filters/VariableTools.java    |   0
 .../us/ihmc/yoVariables/filters/YoMatrix.java |   0
 .../AlphaFilteredRigidBodyTransformTest.java  |   0
 .../filters/AlphaFilteredTuple3DTest.java     |   0
 .../AlphaFilteredYoFramePoint2DTest.java      |   0
 .../AlphaFilteredYoFramePoint3DTest.java      |   0
 .../AlphaFilteredYoFrameQuaternionTest.java   |   0
 .../AlphaFilteredYoFrameVector2DTest.java     |   0
 .../AlphaFilteredYoFrameVector3DTest.java     |   0
 .../filters/EuclidCoreTestMissingTools.java   |   0
 .../FilteredVelocityYoFrameVector2DTest.java  |   0
 .../FilteredVelocityYoFrameVector3DTest.java  |   0
 .../RateLimitedYoFrameOrientationTest.java    |   0
 .../RateLimitedYoFrameQuaternionTest.java     |   0
 .../RunningAverageYoFrameVector3DTest.java    |   0
 .../euclid/filters/YoMatrixTest.java          |   0
 .../AlphaBetaFilteredYoVariableTest.java      |   0
 .../AlphaFilteredWrappingYoVariableTest.java  |   0
 .../filters/AlphaFilteredYoVariableTest.java  |   0
 ...ashCompensatingVelocityYoVariableTest.java |   0
 .../BacklashProcessingYoVariableTest.java     |   0
 .../ButterworthFilteredYoVariableTest.java    |   0
 .../filters/DeadbandedYoVariableTest.java     |   0
 .../filters/DelayedYoBooleanTest.java         |   0
 .../filters/DelayedYoDoubleTest.java          |   0
 .../filters/DeltaLimitedYoVariableTest.java   |   0
 ...ilteredDiscreteVelocityYoVariableTest.java |   0
 .../FilteredVelocityYoVariableTest.java       |   0
 .../FirstOrderFilteredYoDoubleTest.java       |   0
 .../filters/GlitchFilteredYoBooleanTest.java  |   0
 .../filters/MovingAverageYoDoubleTest.java    |   0
 .../filters/RateLimitedYoVariableTest.java    |   0
 .../filters/RunningAverageYoDoubleTest.java   |   0
 .../SecondOrderFilteredYoDoubleTest.java      |   0
 ...leMovingAverageFilteredYoVariableTest.java |   0
 99 files changed, 7 insertions(+), 259 deletions(-)
 delete mode 100644 ihmc-yovariables-filters/LICENSE.txt
 delete mode 100644 ihmc-yovariables-filters/build.gradle.kts
 delete mode 100644 ihmc-yovariables-filters/gradle.properties
 delete mode 100644 ihmc-yovariables-filters/settings.gradle.kts
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/BacklashState.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java (96%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java (95%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/VariableTools.java (100%)
 rename {ihmc-yovariables-filters/src => src}/main/java/us/ihmc/yoVariables/filters/YoMatrix.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2DTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2DTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/EuclidCoreTestMissingTools.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector2DTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector3DTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariableTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariableTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariableTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariableTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariableTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariableTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/DeadbandedYoVariableTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/DelayedYoBooleanTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/DelayedYoDoubleTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariableTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/FilteredVelocityYoVariableTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBooleanTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/RateLimitedYoVariableTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/RunningAverageYoDoubleTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java (100%)
 rename {ihmc-yovariables-filters/src => src}/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java (100%)

diff --git a/build.gradle.kts b/build.gradle.kts
index c21d5973..a7410490 100644
--- a/build.gradle.kts
+++ b/build.gradle.kts
@@ -21,9 +21,11 @@ dependencies {
 
    api("us.ihmc:ihmc-commons:0.32.0")
    api("us.ihmc:euclid-frame:0.21.0")
+//   api("org.ejml:ejml-ddense:0.39")
 }
 
 testDependencies {
    api("us.ihmc:ihmc-commons-testing:0.32.0")
    api("us.ihmc:euclid-test:0.21.0")
+   api("org.apache.commons:commons-math3:3.3")
 }
diff --git a/ihmc-yovariables-filters/LICENSE.txt b/ihmc-yovariables-filters/LICENSE.txt
deleted file mode 100644
index d6456956..00000000
--- a/ihmc-yovariables-filters/LICENSE.txt
+++ /dev/null
@@ -1,202 +0,0 @@
-
-                                 Apache License
-                           Version 2.0, January 2004
-                        http://www.apache.org/licenses/
-
-   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
-
-   1. Definitions.
-
-      "License" shall mean the terms and conditions for use, reproduction,
-      and distribution as defined by Sections 1 through 9 of this document.
-
-      "Licensor" shall mean the copyright owner or entity authorized by
-      the copyright owner that is granting the License.
-
-      "Legal Entity" shall mean the union of the acting entity and all
-      other entities that control, are controlled by, or are under common
-      control with that entity. For the purposes of this definition,
-      "control" means (i) the power, direct or indirect, to cause the
-      direction or management of such entity, whether by contract or
-      otherwise, or (ii) ownership of fifty percent (50%) or more of the
-      outstanding shares, or (iii) beneficial ownership of such entity.
-
-      "You" (or "Your") shall mean an individual or Legal Entity
-      exercising permissions granted by this License.
-
-      "Source" form shall mean the preferred form for making modifications,
-      including but not limited to software source code, documentation
-      source, and configuration files.
-
-      "Object" form shall mean any form resulting from mechanical
-      transformation or translation of a Source form, including but
-      not limited to compiled object code, generated documentation,
-      and conversions to other media types.
-
-      "Work" shall mean the work of authorship, whether in Source or
-      Object form, made available under the License, as indicated by a
-      copyright notice that is included in or attached to the work
-      (an example is provided in the Appendix below).
-
-      "Derivative Works" shall mean any work, whether in Source or Object
-      form, that is based on (or derived from) the Work and for which the
-      editorial revisions, annotations, elaborations, or other modifications
-      represent, as a whole, an original work of authorship. For the purposes
-      of this License, Derivative Works shall not include works that remain
-      separable from, or merely link (or bind by name) to the interfaces of,
-      the Work and Derivative Works thereof.
-
-      "Contribution" shall mean any work of authorship, including
-      the original version of the Work and any modifications or additions
-      to that Work or Derivative Works thereof, that is intentionally
-      submitted to Licensor for inclusion in the Work by the copyright owner
-      or by an individual or Legal Entity authorized to submit on behalf of
-      the copyright owner. For the purposes of this definition, "submitted"
-      means any form of electronic, verbal, or written communication sent
-      to the Licensor or its representatives, including but not limited to
-      communication on electronic mailing lists, source code control systems,
-      and issue tracking systems that are managed by, or on behalf of, the
-      Licensor for the purpose of discussing and improving the Work, but
-      excluding communication that is conspicuously marked or otherwise
-      designated in writing by the copyright owner as "Not a Contribution."
-
-      "Contributor" shall mean Licensor and any individual or Legal Entity
-      on behalf of whom a Contribution has been received by Licensor and
-      subsequently incorporated within the Work.
-
-   2. Grant of Copyright License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      copyright license to reproduce, prepare Derivative Works of,
-      publicly display, publicly perform, sublicense, and distribute the
-      Work and such Derivative Works in Source or Object form.
-
-   3. Grant of Patent License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      (except as stated in this section) patent license to make, have made,
-      use, offer to sell, sell, import, and otherwise transfer the Work,
-      where such license applies only to those patent claims licensable
-      by such Contributor that are necessarily infringed by their
-      Contribution(s) alone or by combination of their Contribution(s)
-      with the Work to which such Contribution(s) was submitted. If You
-      institute patent litigation against any entity (including a
-      cross-claim or counterclaim in a lawsuit) alleging that the Work
-      or a Contribution incorporated within the Work constitutes direct
-      or contributory patent infringement, then any patent licenses
-      granted to You under this License for that Work shall terminate
-      as of the date such litigation is filed.
-
-   4. Redistribution. You may reproduce and distribute copies of the
-      Work or Derivative Works thereof in any medium, with or without
-      modifications, and in Source or Object form, provided that You
-      meet the following conditions:
-
-      (a) You must give any other recipients of the Work or
-          Derivative Works a copy of this License; and
-
-      (b) You must cause any modified files to carry prominent notices
-          stating that You changed the files; and
-
-      (c) You must retain, in the Source form of any Derivative Works
-          that You distribute, all copyright, patent, trademark, and
-          attribution notices from the Source form of the Work,
-          excluding those notices that do not pertain to any part of
-          the Derivative Works; and
-
-      (d) If the Work includes a "NOTICE" text file as part of its
-          distribution, then any Derivative Works that You distribute must
-          include a readable copy of the attribution notices contained
-          within such NOTICE file, excluding those notices that do not
-          pertain to any part of the Derivative Works, in at least one
-          of the following places: within a NOTICE text file distributed
-          as part of the Derivative Works; within the Source form or
-          documentation, if provided along with the Derivative Works; or,
-          within a display generated by the Derivative Works, if and
-          wherever such third-party notices normally appear. The contents
-          of the NOTICE file are for informational purposes only and
-          do not modify the License. You may add Your own attribution
-          notices within Derivative Works that You distribute, alongside
-          or as an addendum to the NOTICE text from the Work, provided
-          that such additional attribution notices cannot be construed
-          as modifying the License.
-
-      You may add Your own copyright statement to Your modifications and
-      may provide additional or different license terms and conditions
-      for use, reproduction, or distribution of Your modifications, or
-      for any such Derivative Works as a whole, provided Your use,
-      reproduction, and distribution of the Work otherwise complies with
-      the conditions stated in this License.
-
-   5. Submission of Contributions. Unless You explicitly state otherwise,
-      any Contribution intentionally submitted for inclusion in the Work
-      by You to the Licensor shall be under the terms and conditions of
-      this License, without any additional terms or conditions.
-      Notwithstanding the above, nothing herein shall supersede or modify
-      the terms of any separate license agreement you may have executed
-      with Licensor regarding such Contributions.
-
-   6. Trademarks. This License does not grant permission to use the trade
-      names, trademarks, service marks, or product names of the Licensor,
-      except as required for reasonable and customary use in describing the
-      origin of the Work and reproducing the content of the NOTICE file.
-
-   7. Disclaimer of Warranty. Unless required by applicable law or
-      agreed to in writing, Licensor provides the Work (and each
-      Contributor provides its Contributions) on an "AS IS" BASIS,
-      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-      implied, including, without limitation, any warranties or conditions
-      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-      PARTICULAR PURPOSE. You are solely responsible for determining the
-      appropriateness of using or redistributing the Work and assume any
-      risks associated with Your exercise of permissions under this License.
-
-   8. Limitation of Liability. In no event and under no legal theory,
-      whether in tort (including negligence), contract, or otherwise,
-      unless required by applicable law (such as deliberate and grossly
-      negligent acts) or agreed to in writing, shall any Contributor be
-      liable to You for damages, including any direct, indirect, special,
-      incidental, or consequential damages of any character arising as a
-      result of this License or out of the use or inability to use the
-      Work (including but not limited to damages for loss of goodwill,
-      work stoppage, computer failure or malfunction, or any and all
-      other commercial damages or losses), even if such Contributor
-      has been advised of the possibility of such damages.
-
-   9. Accepting Warranty or Additional Liability. While redistributing
-      the Work or Derivative Works thereof, You may choose to offer,
-      and charge a fee for, acceptance of support, warranty, indemnity,
-      or other liability obligations and/or rights consistent with this
-      License. However, in accepting such obligations, You may act only
-      on Your own behalf and on Your sole responsibility, not on behalf
-      of any other Contributor, and only if You agree to indemnify,
-      defend, and hold each Contributor harmless for any liability
-      incurred by, or claims asserted against, such Contributor by reason
-      of your accepting any such warranty or additional liability.
-
-   END OF TERMS AND CONDITIONS
-
-   APPENDIX: How to apply the Apache License to your work.
-
-      To apply the Apache License to your work, attach the following
-      boilerplate notice, with the fields enclosed by brackets "[]"
-      replaced with your own identifying information. (Don't include
-      the brackets!)  The text should be enclosed in the appropriate
-      comment syntax for the file format. We also recommend that a
-      file or class name and description of purpose be included on the
-      same "printed page" as the copyright notice for easier
-      identification within third-party archives.
-
-   Copyright [yyyy] [name of copyright owner]
-
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.
diff --git a/ihmc-yovariables-filters/build.gradle.kts b/ihmc-yovariables-filters/build.gradle.kts
deleted file mode 100644
index 34ca6a3c..00000000
--- a/ihmc-yovariables-filters/build.gradle.kts
+++ /dev/null
@@ -1,24 +0,0 @@
-plugins {
-   id("us.ihmc.ihmc-build")
-   id("us.ihmc.ihmc-ci") version "8.3"
-   id("us.ihmc.ihmc-cd") version "1.26"
-   id("us.ihmc.log-tools-plugin") version "0.6.3"
-}
-
-ihmc {
-   loadProductProperties("../product.properties")
-
-   configureDependencyResolution()
-   configurePublications()
-}
-
-mainDependencies {
-   api("us.ihmc:ihmc-yovariables:0.12.2")
-   api("org.ejml:ejml-ddense:0.39")
-   api("us.ihmc:ihmc-commons-utils:source")
-}
-
-testDependencies {
-   api("us.ihmc:ihmc-yovariables-test:0.12.2")
-   api("org.apache.commons:commons-math3:3.3")
-}
diff --git a/ihmc-yovariables-filters/gradle.properties b/ihmc-yovariables-filters/gradle.properties
deleted file mode 100644
index d883b77c..00000000
--- a/ihmc-yovariables-filters/gradle.properties
+++ /dev/null
@@ -1,6 +0,0 @@
-kebabCasedName = ihmc-yovariables-filters
-pascalCasedName = IHMCYoVariablesFilters
-extraSourceSets = ["test"]
-publishUrl = local
-compositeSearchHeight = 2
-excludeFromCompositeBuild = false
\ No newline at end of file
diff --git a/ihmc-yovariables-filters/settings.gradle.kts b/ihmc-yovariables-filters/settings.gradle.kts
deleted file mode 100644
index 31528b14..00000000
--- a/ihmc-yovariables-filters/settings.gradle.kts
+++ /dev/null
@@ -1,23 +0,0 @@
-pluginManagement {
-   plugins {
-      id("us.ihmc.ihmc-build") version "0.29.7"
-   }
-}
-
-buildscript {
-   repositories {
-      maven { url = uri("https://plugins.gradle.org/m2/") }
-      mavenLocal()
-   }
-   dependencies {
-      classpath("us.ihmc:ihmc-build:0.29.7")
-   }
-}
-
-/**
- * Browse source at https://github.com/ihmcrobotics/ihmc-build
- */
-val ihmcSettingsConfigurator = us.ihmc.build.IHMCSettingsConfigurator(settings, logger, extra)
-ihmcSettingsConfigurator.checkRequiredPropertiesAreSet()
-ihmcSettingsConfigurator.configureExtraSourceSets()
-ihmcSettingsConfigurator.findAndIncludeCompositeBuilds()
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java b/src/main/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java
rename to src/main/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java b/src/main/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java
rename to src/main/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java b/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java
rename to src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java b/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java
rename to src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashState.java b/src/main/java/us/ihmc/yoVariables/filters/BacklashState.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/BacklashState.java
rename to src/main/java/us/ihmc/yoVariables/filters/BacklashState.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java b/src/main/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java
rename to src/main/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java b/src/main/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java
rename to src/main/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java
similarity index 96%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java
index c0ac7d96..c6e8d703 100644
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java
+++ b/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java
@@ -1,6 +1,6 @@
 package us.ihmc.yoVariables.filters;
 
-import us.ihmc.commons.AngleTools;
+import us.ihmc.euclid.tools.EuclidCoreTools;
 import us.ihmc.yoVariables.providers.DoubleProvider;
 import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoBoolean;
@@ -123,7 +123,7 @@ public void updateForAngles(double currentPosition)
          set(0.0);
       }
 
-      double difference = AngleTools.computeAngleDifferenceMinusPiToPi(currentPosition, lastPosition.getDoubleValue());
+      double difference = EuclidCoreTools.angleDifferenceMinusPiToPi(currentPosition, lastPosition.getDoubleValue());
 
       updateUsingDifference(difference);
 
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java b/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
rename to src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java b/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
rename to src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java b/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java
rename to src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java b/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java
rename to src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java b/src/main/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java
rename to src/main/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java b/src/main/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java
rename to src/main/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java b/src/main/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java
rename to src/main/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java b/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java
rename to src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java b/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java
rename to src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java b/src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
similarity index 95%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
rename to src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
index 2628b7d9..b6a0b8f0 100644
--- a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
+++ b/src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
@@ -1,12 +1,13 @@
 package us.ihmc.yoVariables.filters;
 
 import org.ejml.data.DMatrixRMaj;
-import org.ejml.dense.row.CommonOps_DDRM;
 
 import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoDouble;
 import us.ihmc.yoVariables.variable.YoInteger;
 
+import java.util.Arrays;
+
 /**
  * Filter the given yoVariable using a moving average filter. This class is NOT REWINDABLE!
  */
@@ -34,7 +35,7 @@ public SimpleMovingAverageFilteredYoVariable(String name, int windowSize, YoDoub
       this.windowSize.set(windowSize);
 
       previousUpdateValues.reshape(windowSize, 1);
-      CommonOps_DDRM.fill(previousUpdateValues, 0.0);
+      Arrays.fill(previousUpdateValues.data, 0.0);
    }
 
    public void setWindowSize(int windowSize)
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/VariableTools.java b/src/main/java/us/ihmc/yoVariables/filters/VariableTools.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/VariableTools.java
rename to src/main/java/us/ihmc/yoVariables/filters/VariableTools.java
diff --git a/ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/YoMatrix.java b/src/main/java/us/ihmc/yoVariables/filters/YoMatrix.java
similarity index 100%
rename from ihmc-yovariables-filters/src/main/java/us/ihmc/yoVariables/filters/YoMatrix.java
rename to src/main/java/us/ihmc/yoVariables/filters/YoMatrix.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransformTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3DTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2DTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2DTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2DTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2DTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3DTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternionTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2DTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2DTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2DTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2DTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3DTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/EuclidCoreTestMissingTools.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/EuclidCoreTestMissingTools.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/EuclidCoreTestMissingTools.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/EuclidCoreTestMissingTools.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector2DTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector2DTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector2DTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector2DTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector3DTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector3DTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector3DTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/FilteredVelocityYoFrameVector3DTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientationTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternionTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java
rename to src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariableTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariableTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariableTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariableTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariableTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariableTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeadbandedYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/DeadbandedYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeadbandedYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/DeadbandedYoVariableTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoBooleanTest.java b/src/test/java/us/ihmc/yoVariables/filters/DelayedYoBooleanTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoBooleanTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/DelayedYoBooleanTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoDoubleTest.java b/src/test/java/us/ihmc/yoVariables/filters/DelayedYoDoubleTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DelayedYoDoubleTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/DelayedYoDoubleTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariableTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredVelocityYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/FilteredVelocityYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FilteredVelocityYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/FilteredVelocityYoVariableTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java b/src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBooleanTest.java b/src/test/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBooleanTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBooleanTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBooleanTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java b/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RateLimitedYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/RateLimitedYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RateLimitedYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/RateLimitedYoVariableTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RunningAverageYoDoubleTest.java b/src/test/java/us/ihmc/yoVariables/filters/RunningAverageYoDoubleTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/RunningAverageYoDoubleTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/RunningAverageYoDoubleTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java b/src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDoubleTest.java
diff --git a/ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java
similarity index 100%
rename from ihmc-yovariables-filters/src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java
rename to src/test/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariableTest.java

From c5e1b80674aae64402e829dee197464f5ea80f00 Mon Sep 17 00:00:00 2001
From: "rgriffin@ihmc.org" 
Date: Thu, 24 Oct 2024 08:40:45 -0500
Subject: [PATCH 44/47] updated the .gitignore

---
 .gitignore | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/.gitignore b/.gitignore
index fffa2131..b63d5f1e 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,9 @@
+# Top level files to ignore
+gradlew
+gradlew.bat
+
 # Top level directories to ignore
+/gradle
 /build
 /classes
 /bin

From c7d39927f85e25a26bb51d271b317cb3f6f334ab Mon Sep 17 00:00:00 2001
From: "rgriffin@ihmc.org" 
Date: Thu, 24 Oct 2024 08:41:59 -0500
Subject: [PATCH 45/47] :arrow_up: ihmc-commons 0.33.0

:arrow_up: euclid 0.22.0
---
 build.gradle.kts | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/build.gradle.kts b/build.gradle.kts
index a7410490..49811202 100644
--- a/build.gradle.kts
+++ b/build.gradle.kts
@@ -19,13 +19,12 @@ dependencies {
    api("net.sf.trove4j:trove4j:3.0.3")
    api("com.sun.xml.bind:jaxb-impl:4.0.5")
 
-   api("us.ihmc:ihmc-commons:0.32.0")
-   api("us.ihmc:euclid-frame:0.21.0")
-//   api("org.ejml:ejml-ddense:0.39")
+   api("us.ihmc:ihmc-commons:0.33.0")
+   api("us.ihmc:euclid-frame:0.22.0")
 }
 
 testDependencies {
-   api("us.ihmc:ihmc-commons-testing:0.32.0")
-   api("us.ihmc:euclid-test:0.21.0")
+   api("us.ihmc:ihmc-commons-testing:0.33.0")
+   api("us.ihmc:euclid-test:0.22.0")
    api("org.apache.commons:commons-math3:3.3")
 }

From 2798421ba44fcd3bf45f6c82397283c66b9659aa Mon Sep 17 00:00:00 2001
From: "rgriffin@ihmc.org" 
Date: Thu, 24 Oct 2024 10:12:45 -0500
Subject: [PATCH 46/47] moved all filters into its own yo variables project

---
 build.gradle.kts                              |   8 ++
 gradle.properties                             |   2 +-
 .../AccelerationLimitedYoFrameVector3D.java   |   0
 .../AlphaFilteredRigidBodyTransform.java      |   0
 .../euclid/filters/AlphaFilteredTuple2D.java  |   0
 .../euclid/filters/AlphaFilteredTuple3D.java  |   0
 .../filters/AlphaFilteredYoFramePoint2D.java  |   0
 .../filters/AlphaFilteredYoFramePoint3D.java  |   0
 .../filters/AlphaFilteredYoFramePose3D.java   |   0
 .../AlphaFilteredYoFrameQuaternion.java       |   0
 .../filters/AlphaFilteredYoFrameVector2D.java |   0
 .../filters/AlphaFilteredYoFrameVector3D.java |   0
 .../AlphaFilteredYoMutableFrameVector3D.java  |   0
 ...shCompensatingVelocityYoFrameVector3D.java |   0
 .../BacklashProcessingYoFrameVector3D.java    |   0
 .../filters/DeadbandedYoFrameVector3D.java    |   0
 ...lteredFiniteDifferenceYoFrameVector2D.java |   0
 ...lteredFiniteDifferenceYoFrameVector3D.java |   0
 ...ferenceAngularVelocityYoFrameVector3D.java |   0
 .../filters/MovingAverageYoFramePoint2D.java  |   0
 .../filters/MovingAverageYoFrameVector2D.java |   0
 .../RateLimitedYoFrameOrientation.java        |   0
 .../filters/RateLimitedYoFramePoint2D.java    |   0
 .../filters/RateLimitedYoFramePoint3D.java    |   0
 .../filters/RateLimitedYoFramePose3D.java     |   0
 .../filters/RateLimitedYoFrameQuaternion.java |   0
 .../filters/RateLimitedYoFrameVector2D.java   |   0
 .../filters/RateLimitedYoFrameVector3D.java   |   0
 .../RateLimitedYoMutableFrameVector3D.java    |   0
 .../RunningAverageYoFrameVector3D.java        |   0
 .../SecondOrderFilteredYoFrameVector3D.java   |   0
 ...eMovingAverageFilteredYoFrameVector3D.java |   0
 .../AccelerationLimitedYoVariable.java        |   0
 .../AlphaBasedOnBreakFrequencyProvider.java   |   0
 .../filters/AlphaBetaFilteredYoVariable.java  |   0
 .../yoVariables/filters/AlphaFilterTools.java |   0
 .../AlphaFilteredWrappingYoVariable.java      |   0
 .../filters/AlphaFilteredYoMatrix.java        |   0
 .../filters/AlphaFilteredYoVariable.java      |   0
 ...acklashCompensatingVelocityYoVariable.java |   0
 .../filters/BacklashProcessingYoVariable.java |   0
 .../yoVariables/filters/BacklashState.java    |   0
 .../ButterworthFilteredYoVariable.java        |   0
 .../filters/DeadbandedYoVariable.java         |   0
 .../yoVariables/filters/DelayedYoBoolean.java |   0
 .../yoVariables/filters/DelayedYoDouble.java  |   0
 .../filters/DeltaLimitedYoVariable.java       |   0
 .../FilteredDiscreteVelocityYoVariable.java   |   0
 .../FilteredFiniteDifferenceYoVariable.java   |   0
 .../FirstOrderBandPassFilteredYoDouble.java   |   0
 .../filters/FirstOrderFilteredYoDouble.java   |   0
 .../filters/GlitchFilteredYoBoolean.java      |   0
 .../filters/GlitchFilteredYoInteger.java      |   0
 .../filters/JerkLimitedYoDouble.java          |   0
 .../filters/MovingAverageYoDouble.java        |   0
 .../filters/ProcessingYoVariable.java         |   0
 .../filters/RateLimitedYoVariable.java        |   0
 .../filters/RunningAverageYoDouble.java       |   0
 .../filters/SecondOrderFilteredYoDouble.java  |   0
 ...condOrderFilteredYoVariableParameters.java |   0
 ...SimpleMovingAverageFilteredYoVariable.java |   0
 .../yoVariables/filters/VariableTools.java    |   0
 .../us/ihmc/yoVariables/filters/YoMatrix.java |   0
 .../us/ihmc/yoVariables/math/YoMatrix.java    |  72 ++++++++---
 .../RunningAverageYoFrameVector3DTest.java    |   4 +-
 .../euclid/filters/YoMatrixTest.java          | 115 ------------------
 ...ilteredDiscreteVelocityYoVariableTest.java |   4 +-
 .../filters/MovingAverageYoDoubleTest.java    |   4 +-
 .../ihmc/yoVariables/math/YoMatrixTest.java   | 104 ++++++++++++++++
 69 files changed, 177 insertions(+), 136 deletions(-)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/BacklashState.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/VariableTools.java (100%)
 rename src/{main => filters}/java/us/ihmc/yoVariables/filters/YoMatrix.java (100%)

diff --git a/build.gradle.kts b/build.gradle.kts
index 49811202..2c095026 100644
--- a/build.gradle.kts
+++ b/build.gradle.kts
@@ -23,7 +23,15 @@ dependencies {
    api("us.ihmc:euclid-frame:0.22.0")
 }
 
+filtersDependencies {
+   api(ihmc.sourceSetProject("main"))
+   api("org.ejml:ejml-ddense:0.39");
+}
+
 testDependencies {
+   api(ihmc.sourceSetProject("main"))
+   api(ihmc.sourceSetProject("filters"))
+
    api("us.ihmc:ihmc-commons-testing:0.33.0")
    api("us.ihmc:euclid-test:0.22.0")
    api("org.apache.commons:commons-math3:3.3")
diff --git a/gradle.properties b/gradle.properties
index 514abe04..e6b42517 100644
--- a/gradle.properties
+++ b/gradle.properties
@@ -1,4 +1,4 @@
 title = IHMC YoVariables
-extraSourceSets = ["test"]
+extraSourceSets = ["filters", "test"]
 compositeSearchHeight = 0
 excludeFromCompositeBuild = false
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint2D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePoint3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFramePose3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameQuaternion.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector2D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredYoMutableFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/BacklashCompensatingVelocityYoFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/BacklashProcessingYoFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/DeadbandedYoFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector2D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java
rename to src/filters/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java
rename to src/filters/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java
rename to src/filters/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java
rename to src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/BacklashProcessingYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/BacklashState.java b/src/filters/java/us/ihmc/yoVariables/filters/BacklashState.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/BacklashState.java
rename to src/filters/java/us/ihmc/yoVariables/filters/BacklashState.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/ButterworthFilteredYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java b/src/filters/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java
rename to src/filters/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java
rename to src/filters/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/FilteredFiniteDifferenceYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
rename to src/filters/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
rename to src/filters/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java b/src/filters/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java
rename to src/filters/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java b/src/filters/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java
rename to src/filters/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java
rename to src/filters/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java
rename to src/filters/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java
rename to src/filters/java/us/ihmc/yoVariables/filters/RunningAverageYoDouble.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java
rename to src/filters/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoDouble.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java b/src/filters/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java
rename to src/filters/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
rename to src/filters/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/VariableTools.java b/src/filters/java/us/ihmc/yoVariables/filters/VariableTools.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/VariableTools.java
rename to src/filters/java/us/ihmc/yoVariables/filters/VariableTools.java
diff --git a/src/main/java/us/ihmc/yoVariables/filters/YoMatrix.java b/src/filters/java/us/ihmc/yoVariables/filters/YoMatrix.java
similarity index 100%
rename from src/main/java/us/ihmc/yoVariables/filters/YoMatrix.java
rename to src/filters/java/us/ihmc/yoVariables/filters/YoMatrix.java
diff --git a/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java b/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java
index 1c599c94..47a3599b 100644
--- a/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java
+++ b/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java
@@ -144,14 +144,38 @@ public YoMatrix(String name, String description, int maxNumberOfRows, int maxNum
          {
             switch (checkNames(rowNames, columnNames))
             {
-               case NONE -> variables[row][column] = new YoDouble(name + "_" + row + "_" + column, description, registry);
-               case ROWS -> variables[row][column] = new YoDouble(name + rowNames[row], description, registry);
-               case ROWS_AND_COLUMNS -> variables[row][column] = new YoDouble(name + rowNames[row] + columnNames[column], description, registry);
+               case NONE -> variables[row][column] = new YoDouble(getFieldName(name, row, column), description, registry);
+               case ROWS -> variables[row][column] = new YoDouble(getFieldName(name, rowNames[row], ""), description, registry);
+               case ROWS_AND_COLUMNS -> variables[row][column] = new YoDouble(getFieldName(name, rowNames[row], columnNames[column]), description, registry);
             }
          }
       }
    }
 
+   /**
+    * Convenience provider for getting the field name of an individual cell in the matrix. This can be used if retrieving the variable name from a registry.
+    * @param prefix name of the entire matrix
+    * @param row    row in question
+    * @param column column in question
+    * @return String describing the name.
+    */
+   public static String getFieldName(String prefix, int row, int column)
+   {
+      return getFieldName(prefix, "_" + row, "_" + column);
+   }
+
+   /**
+    * Convenience provider for getting the field name of an individual cell in the matrix. This can be used if retrieving the variable name from a registry.
+    * @param prefix     name of the entire matrix
+    * @param rowName    row name in question
+    * @param columnName column name in question
+    * @return String describing the name.
+    */
+   public static String getFieldName(String prefix, String rowName, String columnName)
+   {
+      return prefix + rowName + columnName;
+   }
+
    /**
     * Enum used to determine what names have been provided to the YoMatrix.
     */
@@ -212,7 +236,7 @@ public void scale(double scale)
       {
          for (int col = 0; col < getNumCols(); col++)
          {
-            unsafe_set(row, col, unsafe_get(row, col) * scale);
+            unsafe_set(row, col, unsafe_get(row, col) * scale, false);
          }
       }
    }
@@ -233,7 +257,7 @@ public void scale(double scale, DMatrix matrix)
       {
          for (int col = 0; col < getNumCols(); col++)
          {
-            unsafe_set(row, col, matrix.unsafe_get(row, col) * scale);
+            unsafe_set(row, col, matrix.unsafe_get(row, col) * scale, false);
          }
       }
    }
@@ -283,7 +307,7 @@ public void add(double alpha, DMatrix a, double beta, DMatrix b)
       {
          for (int col = 0; col < getNumCols(); col++)
          {
-            unsafe_set(row, col, alpha * a.unsafe_get(row, col) + beta * b.unsafe_get(row, col));
+            unsafe_set(row, col, alpha * a.unsafe_get(row, col) + beta * b.unsafe_get(row, col), false);
          }
       }
    }
@@ -314,7 +338,7 @@ public void addEquals(double alpha, DMatrix a)
       {
          for (int col = 0; col < getNumCols(); col++)
          {
-            unsafe_set(row, col, unsafe_get(row, col) + alpha * a.unsafe_get(row, col));
+            unsafe_set(row, col, unsafe_get(row, col) + alpha * a.unsafe_get(row, col), false);
          }
       }
    }
@@ -398,7 +422,7 @@ else if (numRows < 0 || numCols < 0)
       {
          for (int col = numCols; col < maxNumberOfColumns; col++)
          {
-            unsafe_set(row, col, Double.NaN);
+            unsafe_set(row, col, Double.NaN, false);
          }
       }
 
@@ -406,7 +430,7 @@ else if (numRows < 0 || numCols < 0)
       {
          for (int col = 0; col < maxNumberOfColumns; col++)
          {
-            unsafe_set(row, col, Double.NaN);
+            unsafe_set(row, col, Double.NaN, false);
          }
       }
    }
@@ -426,10 +450,30 @@ public void set(int row, int col, double val)
       unsafe_set(row, col, val);
    }
 
+   /**
+    * Sets the variable contained at the sepcified index, but does not check whether the field is in bounds. This will call any attached yo variable listeners.
+    * If you want to avoid calling these listeners, please call {@link #unsafe_set(int, int, double, boolean)}.
+    * @param row Matrix element's row index.
+    * @param col Matrix element's column index.
+    * @param val The element's new value.
+    */
    @Override
    public void unsafe_set(int row, int col, double val)
    {
-      variables[row][col].set(val);
+      unsafe_set(row, col, val, true);
+   }
+
+   /**
+    * Sets the variable contained at the sepcified index, but does not check whether the field is in bounds. Optionally allows you to avoid notifying
+    * listeners.
+    * @param row             Matrix element's row index.
+    * @param col             Matrix element's column index.
+    * @param val             The element's new value.
+    * @param notifyListeners trigger for whether to notify attached yo variable listeners.
+    */
+   public void unsafe_set(int row, int col, double val, boolean notifyListeners)
+   {
+      variables[row][col].set(val, notifyListeners);
    }
 
    /**
@@ -450,7 +494,7 @@ public void set(Matrix original)
          {
             for (int col = 0; col < getNumCols(); col++)
             {
-               unsafe_set(row, col, otherMatrix.unsafe_get(row, col));
+               unsafe_set(row, col, otherMatrix.unsafe_get(row, col), false);
             }
          }
       }
@@ -473,7 +517,7 @@ public void setToNaN(int numRows, int numCols)
       {
          for (int col = 0; col < numCols; col++)
          {
-            unsafe_set(row, col, Double.NaN);
+            unsafe_set(row, col, Double.NaN, false);
          }
       }
    }
@@ -507,9 +551,9 @@ public void zero()
          for (int col = 0; col < maxNumberOfColumns; col++)
          {
             if (row < getNumRows() && col < getNumCols())
-               unsafe_set(row, col, 0.0);
+               unsafe_set(row, col, 0.0, false);
             else
-               unsafe_set(row, col, Double.NaN);
+               unsafe_set(row, col, Double.NaN, false);
          }
       }
    }
diff --git a/src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java
index 5ff399ef..cad057e3 100644
--- a/src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java
+++ b/src/test/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3DTest.java
@@ -1,7 +1,5 @@
 package us.ihmc.yoVariables.euclid.filters;
 
-import static org.junit.jupiter.api.Assertions.*;
-
 import java.util.Random;
 
 import org.apache.commons.math3.stat.descriptive.moment.Mean;
@@ -9,6 +7,8 @@
 
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
 
+import static org.junit.jupiter.api.Assertions.*;
+
 public class RunningAverageYoFrameVector3DTest
 {
    @Test
diff --git a/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java
index ab37128f..9847d094 100644
--- a/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java
+++ b/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java
@@ -15,122 +15,7 @@
 
 public class YoMatrixTest
 {
-   @Test
-   public void testSimpleYoMatrixExample()
-   {
-      int maxNumberOfRows = 4;
-      int maxNumberOfColumns = 8;
-      YoRegistry registry = new YoRegistry("testRegistry");
-      YoMatrix yoMatrix = new YoMatrix("testMatrix", maxNumberOfRows, maxNumberOfColumns, registry);
-      assertEquals(maxNumberOfRows, yoMatrix.getNumRows());
-      assertEquals(maxNumberOfColumns, yoMatrix.getNumCols());
-
-      DMatrixRMaj denseMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns);
-      yoMatrix.reshape(maxNumberOfRows, maxNumberOfColumns);
-      yoMatrix.zero();
-      yoMatrix.get(denseMatrix);
-
-      DMatrixRMaj zeroMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns);
-      EjmlUnitTests.assertEquals(zeroMatrix, denseMatrix, 1e-10);
-
-      Random random = new Random(1984L);
-
-      DMatrixRMaj randomMatrix = RandomMatrices_DDRM.rectangle(maxNumberOfRows, maxNumberOfColumns, random);
-      yoMatrix.set(randomMatrix);
-
-      DMatrixRMaj checkMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns);
-      yoMatrix.get(checkMatrix);
-
-      EjmlUnitTests.assertEquals(randomMatrix, checkMatrix, 1e-10);
-
-      assertEquals(registry.findVariable(YoMatrix.getFieldName("testMatrix", 0, 0)).getValueAsDouble(), checkMatrix.get(0, 0), 1e-10);
-   }
-
-   @Test
-   public void testYoMatrixDimensioning()
-   {
-      int maxNumberOfRows = 4;
-      int maxNumberOfColumns = 8;
-      String name = "testMatrix";
-      
-      YoRegistry registry = new YoRegistry("testRegistry");
-      YoMatrix yoMatrix = new YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry);
-
-      int smallerRows = maxNumberOfRows - 2;
-      int smallerColumns = maxNumberOfColumns - 3;
-      DMatrixRMaj denseMatrix = new DMatrixRMaj(smallerRows, smallerColumns);
-
-      try
-      {
-         yoMatrix.get(denseMatrix);
-         fail("Should throw an exception if the size isn't right!");
-      }
-      catch (Exception e)
-      {
-      }
-
-      yoMatrix.reshape(maxNumberOfRows, maxNumberOfColumns);
-      yoMatrix.zero();
-      yoMatrix.getAndReshape(denseMatrix);
-      DMatrixRMaj zeroMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns);
-      EjmlUnitTests.assertEquals(zeroMatrix, denseMatrix, 1e-10);
-      assertEquals(maxNumberOfRows, denseMatrix.getNumRows());
-      assertEquals(maxNumberOfColumns, denseMatrix.getNumCols());
-
-      assertEquals(maxNumberOfRows, yoMatrix.getNumRows());
-      assertEquals(maxNumberOfColumns, yoMatrix.getNumCols());
-
-      Random random = new Random(1984L);
-
-      DMatrixRMaj randomMatrix = RandomMatrices_DDRM.rectangle(maxNumberOfRows, maxNumberOfColumns, random);
-      yoMatrix.set(randomMatrix);
-
-      DMatrixRMaj checkMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns);
-      yoMatrix.get(checkMatrix);
 
-      EjmlUnitTests.assertEquals(randomMatrix, checkMatrix, 1e-10);
-
-      DMatrixRMaj smallerMatrix = RandomMatrices_DDRM.rectangle(smallerRows, smallerColumns, random);
-      yoMatrix.set(smallerMatrix);
-
-      assertEquals(smallerRows, smallerMatrix.getNumRows());
-      assertEquals(smallerColumns, smallerMatrix.getNumCols());
-
-      assertEquals(smallerRows, yoMatrix.getNumRows());
-      assertEquals(smallerColumns, yoMatrix.getNumCols());
-
-      DMatrixRMaj checkMatrix2 = new DMatrixRMaj(1, 1);
-      yoMatrix.getAndReshape(checkMatrix2);
-
-      EjmlUnitTests.assertEquals(smallerMatrix, checkMatrix2, 1e-10);
-
-      checkMatrixYoVariablesEqualsCheckMatrixAndOutsideValuesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, checkMatrix2, registry);
-   }
-   
-   @Test
-   public void testYoMatrixSetToZero()
-   {
-      int maxNumberOfRows = 4;
-      int maxNumberOfColumns = 8;
-      String name = "testMatrixForZero";
-      
-      YoRegistry registry = new YoRegistry("testRegistry");
-      YoMatrix yoMatrix = new YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry);
-      
-      Random random = new Random(1984L);
-
-      DMatrixRMaj randomMatrix = RandomMatrices_DDRM.rectangle(maxNumberOfRows, maxNumberOfColumns, random);
-      yoMatrix.set(randomMatrix);
-      
-      int numberOfRows = 2;
-      int numberOfColumns = 6;
-      yoMatrix.reshape(numberOfRows, numberOfColumns);
-      yoMatrix.zero();
-      
-      DMatrixRMaj zeroMatrix = new DMatrixRMaj(numberOfRows, numberOfColumns);
-      zeroMatrix.zero();
-      checkMatrixYoVariablesEqualsCheckMatrixAndOutsideValuesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, zeroMatrix, registry);  
-   }
    
    @Test
    public void testYoMatrixSetTooBig()
diff --git a/src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java b/src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java
index ead6291e..0fa77c87 100644
--- a/src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java
+++ b/src/test/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariableTest.java
@@ -1,12 +1,12 @@
 package us.ihmc.yoVariables.filters;
 
-import static org.junit.jupiter.api.Assertions.*;
-
 import org.junit.jupiter.api.Test;
 
 import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoDouble;
 
+import static org.junit.jupiter.api.Assertions.*;
+
 public class FilteredDiscreteVelocityYoVariableTest
 {
 
diff --git a/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java b/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java
index 4975fecd..cefb7be0 100644
--- a/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java
+++ b/src/test/java/us/ihmc/yoVariables/filters/MovingAverageYoDoubleTest.java
@@ -1,7 +1,5 @@
 package us.ihmc.yoVariables.filters;
 
-import static org.junit.jupiter.api.Assertions.*;
-
 import java.util.Random;
 
 import org.junit.jupiter.api.Test;
@@ -9,6 +7,8 @@
 import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoDouble;
 
+import static org.junit.jupiter.api.Assertions.*;
+
 public class MovingAverageYoDoubleTest
 {
    private final Random rng = new Random();
diff --git a/src/test/java/us/ihmc/yoVariables/math/YoMatrixTest.java b/src/test/java/us/ihmc/yoVariables/math/YoMatrixTest.java
index a744b96a..c4a6df2f 100644
--- a/src/test/java/us/ihmc/yoVariables/math/YoMatrixTest.java
+++ b/src/test/java/us/ihmc/yoVariables/math/YoMatrixTest.java
@@ -1,5 +1,6 @@
 package us.ihmc.yoVariables.math;
 
+import org.ejml.EjmlUnitTests;
 import org.ejml.data.DMatrixRMaj;
 import org.ejml.dense.row.CommonOps_DDRM;
 import org.ejml.dense.row.RandomMatrices_DDRM;
@@ -47,6 +48,39 @@ public void testSimpleYoMatrixRefactorExample()
       assertEquals(smallerRowSize * smallerColumnSize, matrix.getNumElements());
    }
 
+   @Test
+   public void testSimpleYoMatrixExample()
+   {
+      int maxNumberOfRows = 4;
+      int maxNumberOfColumns = 8;
+      YoRegistry registry = new YoRegistry("testRegistry");
+      us.ihmc.yoVariables.filters.YoMatrix yoMatrix = new us.ihmc.yoVariables.filters.YoMatrix("testMatrix", maxNumberOfRows, maxNumberOfColumns, registry);
+      assertEquals(maxNumberOfRows, yoMatrix.getNumRows());
+      assertEquals(maxNumberOfColumns, yoMatrix.getNumCols());
+
+      DMatrixRMaj denseMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns);
+      yoMatrix.reshape(maxNumberOfRows, maxNumberOfColumns);
+      yoMatrix.zero();
+      yoMatrix.get(denseMatrix);
+
+      DMatrixRMaj zeroMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns);
+      EjmlUnitTests.assertEquals(zeroMatrix, denseMatrix, 1e-10);
+
+      Random random = new Random(1984L);
+
+      DMatrixRMaj randomMatrix = RandomMatrices_DDRM.rectangle(maxNumberOfRows, maxNumberOfColumns, random);
+      yoMatrix.set(randomMatrix);
+
+      DMatrixRMaj checkMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns);
+      yoMatrix.get(checkMatrix);
+
+      EjmlUnitTests.assertEquals(randomMatrix, checkMatrix, 1e-10);
+
+      assertEquals(registry.findVariable(us.ihmc.yoVariables.filters.YoMatrix.getFieldName("testMatrix", 0, 0)).getValueAsDouble(), checkMatrix.get(0, 0), 1e-10);
+   }
+
+
+
    @Test
    public void testConstructorsWithNamesAndDescriptions()
    {
@@ -278,6 +312,76 @@ public void testScale()
       }
    }
 
+
+   @Test
+   public void testYoMatrixSetTooBig()
+   {
+      int maxNumberOfRows = 4;
+      int maxNumberOfColumns = 8;
+      String name = "testMatrix";
+      YoRegistry registry = new YoRegistry("testRegistry");
+      us.ihmc.yoVariables.filters.YoMatrix yoMatrix = new us.ihmc.yoVariables.filters.YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry);
+
+      DMatrixRMaj tooBigMatrix = new DMatrixRMaj(maxNumberOfRows + 1, maxNumberOfColumns);
+
+      try
+      {
+         yoMatrix.set(tooBigMatrix);
+         fail("Too Big");
+      }
+      catch (RuntimeException e)
+      {
+      }
+
+      tooBigMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns + 1);
+
+      try
+      {
+         yoMatrix.set(tooBigMatrix);
+         fail("Too Big");
+      }
+      catch (RuntimeException e)
+      {
+      }
+
+      // Test a 0 X Big Matrix
+      DMatrixRMaj okMatrix = new DMatrixRMaj(0, maxNumberOfColumns + 10);
+      yoMatrix.set(okMatrix);
+      assertMatrixYoVariablesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, registry);
+
+      DMatrixRMaj checkMatrix = new DMatrixRMaj(1, 1);
+      yoMatrix.getAndReshape(checkMatrix);
+
+      assertEquals(0, checkMatrix.getNumRows());
+      assertEquals(maxNumberOfColumns + 10, checkMatrix.getNumCols());
+
+      // Test a Big X 0 Matrix
+
+      okMatrix = new DMatrixRMaj(maxNumberOfRows + 10, 0);
+      yoMatrix.set(okMatrix);
+      assertMatrixYoVariablesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, registry);
+
+      checkMatrix = new DMatrixRMaj(1, 1);
+      yoMatrix.getAndReshape(checkMatrix);
+
+      assertEquals(maxNumberOfRows + 10, checkMatrix.getNumRows());
+      assertEquals(0, checkMatrix.getNumCols());
+
+   }
+
+
+   private static void assertMatrixYoVariablesAreNaN(String name, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry)
+   {
+      for (int row = 0; row < maxNumberOfRows; row++)
+      {
+         for (int column = 0; column < maxNumberOfColumns; column++)
+         {
+            YoDouble variable = (YoDouble) registry.findVariable(us.ihmc.yoVariables.filters.YoMatrix.getFieldName(name, row, column));
+            assertTrue(Double.isNaN(variable.getDoubleValue()));
+         }
+      }
+   }
+
    @Test
    public void testScaleFailureCases()
    {

From 502e05bf331a79f7b3fb06e21dd22c29b02ea38a Mon Sep 17 00:00:00 2001
From: "rgriffin@ihmc.org" 
Date: Thu, 24 Oct 2024 13:51:20 -0500
Subject: [PATCH 47/47] fixed the high and band pass filters

---
 .../FirstOrderBandPassFilteredYoDouble.java   | 162 +++---------------
 .../filters/FirstOrderFilteredYoDouble.java   |  31 +++-
 .../FirstOrderFilteredYoDoubleTest.java       |   1 -
 3 files changed, 51 insertions(+), 143 deletions(-)

diff --git a/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
index 626d02d0..3599edc7 100644
--- a/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
+++ b/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
@@ -5,142 +5,51 @@
 import us.ihmc.yoVariables.variable.YoBoolean;
 import us.ihmc.yoVariables.variable.YoDouble;
 
-public class FirstOrderBandPassFilteredYoDouble extends YoDouble
+public class FirstOrderBandPassFilteredYoDouble  extends FirstOrderFilteredYoDouble
 {
-   private final YoBoolean hasBeenCalled;
+   private boolean hasBeenCalled = false;
 
-   private double filterUpdateTimeOld;
-   private final FirstOrderFilterType filterType;
+   private final FirstOrderFilteredYoDouble highPassFilteredInput;
 
-   private final YoDouble firstCutoffFrequencyHz;
-   private final YoDouble secondCutoffFrequencyHz;
-
-   private final DoubleProvider yoTime;
-   private double dt;
-
-   public enum FirstOrderFilterType
+   public FirstOrderBandPassFilteredYoDouble(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz,
+                                               YoDouble yoTime, YoRegistry registry)
    {
-      NOTCH, BAND
-   }
+      super(name, description, maxPassThroughFrequency_Hz, yoTime, FirstOrderFilteredYoDouble.FirstOrderFilterType.LOW_PASS, registry);
 
-   public FirstOrderBandPassFilteredYoDouble(String name,
-                                             String description,
-                                             double minPassThroughFrequency_Hz,
-                                             double maxPassThroughFrequency_Hz,
-                                             DoubleProvider yoTime,
-                                             FirstOrderFilterType filterType,
-                                             YoRegistry registry)
-   {
-      this(name, description, minPassThroughFrequency_Hz, maxPassThroughFrequency_Hz, yoTime, 0.0, filterType, registry);
-   }
+      this.highPassFilteredInput = new FirstOrderFilteredYoDouble(name + "HighPassFilteredOnly", description, minPassThroughFrequency_Hz, yoTime,
+                                                                    FirstOrderFilteredYoDouble.FirstOrderFilterType.HIGH_PASS, registry);
 
-   public FirstOrderBandPassFilteredYoDouble(String name,
-                                             String description,
-                                             double minPassThroughFrequency_Hz,
-                                             double maxPassThroughFrequency_Hz,
-                                             double dt,
-                                             FirstOrderFilterType filterType,
-                                             YoRegistry registry)
-   {
-      this(name, description, minPassThroughFrequency_Hz, maxPassThroughFrequency_Hz, null, dt, filterType, registry);
+      setPassBand(minPassThroughFrequency_Hz, maxPassThroughFrequency_Hz);
    }
 
-   private FirstOrderBandPassFilteredYoDouble(String name,
-                                              String description,
-                                              double minPassThroughFrequency_Hz,
-                                              double maxPassThroughFrequency_Hz,
-                                              DoubleProvider yoTime,
-                                              double dt,
-                                              FirstOrderFilterType filterType,
-                                              YoRegistry registry)
+   public FirstOrderBandPassFilteredYoDouble(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz,
+                                               double DT, YoRegistry registry)
    {
-      super(name, description, registry);
-
-      String firstCutoffFrequencyName, secondCuttoffFrequencyName;
-      switch (filterType)
-      {
-         case NOTCH:
-            firstCutoffFrequencyName = name + "_NotchPassStart_Hz";
-            secondCuttoffFrequencyName = name + "_NotchPassEnd_Hz";
-            break;
-         case BAND:
-            firstCutoffFrequencyName = name + "_BandPassStart_Hz";
-            secondCuttoffFrequencyName = name + "_BandPassEnd_Hz";
-            break;
-         default:
-            throw new RuntimeException("Must specify filter type notch or break");
-      }
-
-      hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(name, "", registry);
-      hasBeenCalled.set(false);
-      this.firstCutoffFrequencyHz = new YoDouble(firstCutoffFrequencyName, registry);
-      this.firstCutoffFrequencyHz.set(minPassThroughFrequency_Hz);
-      this.secondCutoffFrequencyHz = new YoDouble(secondCuttoffFrequencyName, registry);
-      this.secondCutoffFrequencyHz.set(maxPassThroughFrequency_Hz);
+      super(name, description, maxPassThroughFrequency_Hz, DT, FirstOrderFilteredYoDouble.FirstOrderFilterType.LOW_PASS, registry);
 
-      this.yoTime = yoTime;
-      this.dt = dt;
-
-      this.filterType = filterType;
+      this.highPassFilteredInput = new FirstOrderFilteredYoDouble(name + "HighPassFilteredOnly", description, minPassThroughFrequency_Hz, DT,
+                                                                    FirstOrderFilteredYoDouble.FirstOrderFilterType.HIGH_PASS, registry);
    }
 
    private void checkPassband(double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz)
    {
       if (minPassThroughFrequency_Hz > maxPassThroughFrequency_Hz)
       {
-         throw new RuntimeException(
-               "minPassThroughFrequency [ " + minPassThroughFrequency_Hz + " ] > maxPassThroughFrequency [ " + maxPassThroughFrequency_Hz + " ]");
+         throw new RuntimeException("minPassThroughFrequency [ " + minPassThroughFrequency_Hz + " ] > maxPassThroughFrequency [ " + maxPassThroughFrequency_Hz
+                                    + " ]");
       }
    }
 
-   public void reset()
-   {
-      hasBeenCalled.set(false);
-   }
-
    public void update(double filterInput)
    {
-      if (!hasBeenCalled.getBooleanValue())
+      if (!hasBeenCalled)
       {
-         hasBeenCalled.set(true);
+         hasBeenCalled = true;
          this.set(filterInput);
       }
       else
       {
-         if (yoTime != null)
-         {
-            double timeSinceLastUpdate = yoTime.getValue() - filterUpdateTimeOld;
-
-            if (timeSinceLastUpdate > 0.0)
-            {
-               dt = timeSinceLastUpdate;
-            }
-            else
-            {
-               reset();
-               //                  throw new RuntimeException("Computed step size, DT must be greater than zero.   DT = " + dt + ".  Current time = " + yoTime.getDoubleValue() + ", previous update time = " + filterUpdateTimeOld);
-            }
-         }
-         double filterOutput;
-
-         switch (filterType)
-         {
-            case NOTCH:
-               filterOutput = applyNotchFilter(filterInput, dt);
-               break;
-            case BAND:
-               filterOutput = applyBandFilter(filterInput, dt);
-               break;
-            default:
-               throw new RuntimeException("The first order filter must be either a high pass or low pass filter, it cannot be " + filterType);
-         }
-
-         this.set(filterOutput);
-      }
-
-      if (yoTime != null)
-      {
-         filterUpdateTimeOld = yoTime.getValue();
+         updateHighPassFilterAndThenLowPassFilterThat(filterInput);
       }
    }
 
@@ -148,37 +57,14 @@ public void setPassBand(double minPassThroughFreqHz, double maxPassThroughFreqHz
    {
       checkPassband(minPassThroughFreqHz, maxPassThroughFreqHz);
 
-      firstCutoffFrequencyHz.set(minPassThroughFreqHz);
-      secondCutoffFrequencyHz.set(maxPassThroughFreqHz);
-   }
-
-   private double applyNotchFilter(double filterInput, double dt)
-   {
-      double lowFiltered = applyLowPassFilter(filterInput, firstCutoffFrequencyHz.getDoubleValue(), dt);
-      double highFiltered = applyHighPassFilter(filterInput, secondCutoffFrequencyHz.getDoubleValue(), dt);
-
-      return lowFiltered + highFiltered;
-   }
-
-   private double applyBandFilter(double filterInput, double dt)
-   {
-      double lowFiltered = applyLowPassFilter(filterInput, firstCutoffFrequencyHz.getDoubleValue(), dt);
-      double highFiltered = applyHighPassFilter(filterInput, secondCutoffFrequencyHz.getDoubleValue(), dt);
-
-      return filterInput - lowFiltered - highFiltered;
-   }
-
-   private double applyLowPassFilter(double filterInput, double breakFrequency, double dt)
-   {
-      double alpha = AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(dt, breakFrequency);
-
-      return alpha * this.getDoubleValue() + (1.0 - alpha) * filterInput;
+      highPassFilteredInput.setCutoffFrequencyHz(minPassThroughFreqHz);
+      this.setCutoffFrequencyHz(maxPassThroughFreqHz);
    }
 
-   private double applyHighPassFilter(double filterInput, double breakFrequency, double dt)
+   private void updateHighPassFilterAndThenLowPassFilterThat(double filterInput)
    {
-      double lowPassValue = applyLowPassFilter(filterInput, breakFrequency, dt);
+      this.highPassFilteredInput.update(filterInput);
 
-      return filterInput - lowPassValue;
+      super.update(highPassFilteredInput.getDoubleValue());
    }
 }
\ No newline at end of file
diff --git a/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
index 89db7267..71e28fd3 100644
--- a/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
+++ b/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
@@ -8,11 +8,12 @@ public class FirstOrderFilteredYoDouble extends YoDouble
 {
    public enum FirstOrderFilterType
    {
-      LOW_PASS, NOTCH, BAND, HIGH_PASS
+      LOW_PASS, HIGH_PASS
    }
 
    private boolean hasBeenCalled = false;
 
+   private double filterInputOld;
    private double filterUpdateTimeOld;
 
    private final YoDouble cutoffFrequencyHz;
@@ -65,18 +66,38 @@ public FirstOrderFilteredYoDouble(String name,
 
    private double computeLowPassUpdate(double filterInput, double dt)
    {
-      double alpha = AlphaFilterTools.computeAlphaGivenBreakFrequencyProperly(dt, cutoffFrequencyHz.getDoubleValue());
+      double alpha = computeAlpha(dt, cutoffFrequencyHz.getDoubleValue());
 
       return alpha * this.getDoubleValue() + (1.0 - alpha) * filterInput;
    }
 
    private double computeHighPassUpdate(double filterInput, double dt)
    {
-      double lowPassValue = computeLowPassUpdate(filterInput, dt);
+      double alpha = computeAlpha(dt, cutoffFrequencyHz.getDoubleValue());
 
-      return filterInput - lowPassValue;
+      double ret = alpha * (this.getDoubleValue() + filterInput - filterInputOld);
+      return ret;
    }
 
+   private double computeAlpha(double dt, double cutoffFrequencyHz)
+{
+   if (cutoffFrequencyHz <= 0.0)
+   {
+      throw new RuntimeException("Cutoff Frequency must be greater than zero.  Cutoff = " + cutoffFrequencyHz);
+   }
+
+   double cutoff_radPerSec = cutoffFrequencyHz * 2.0 * Math.PI;
+   double RC = 1.0 / cutoff_radPerSec;
+   double alpha = RC / (RC + dt);  // alpha decreases with increasing cutoff frequency
+
+   if (alpha <= 0 || alpha >= 1.0 && dt != 0.0)
+   {
+      throw new RuntimeException("Alpha value must be between 0 and 1.  Alpha = " + alpha);
+   }
+
+   return alpha;
+}
+
    public void reset()
    {
       hasBeenCalled = false;
@@ -93,6 +114,7 @@ public void update(double filterInput)
       {
          hasBeenCalled = true;
 
+         filterInputOld = 0.0;
          filterUpdateTimeOld = 0.0;
 
          this.set(filterInput);
@@ -131,6 +153,7 @@ public void update(double filterInput)
          this.set(filterOutput);
       }
 
+      filterInputOld = filterInput;
 
       if (yoTime != null)
       {
diff --git a/src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java b/src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java
index b97a1485..2f8444c0 100644
--- a/src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java
+++ b/src/test/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDoubleTest.java
@@ -88,7 +88,6 @@ public void testBandPassAttenuationForSinusoidalInput()
                                                                                                              a,
                                                                                                              b,
                                                                                                              yoTime,
-                                                                                                             FirstOrderBandPassFilteredYoDouble.FirstOrderFilterType.BAND,
                                                                                                              registry);
 
       while (filterAttenuation > 0.1 && a > 0.0 && b > 0.0)