+ * {@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/src/filters/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/FilteredFiniteDifferenceYoFrameVector3D.java
new file mode 100644
index 00000000..557eff95
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/FiniteDifferenceAngularVelocityYoFrameVector3D.java
new file mode 100644
index 00000000..1576414a
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFramePoint2D.java
new file mode 100644
index 00000000..e06c568e
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/MovingAverageYoFrameVector2D.java
new file mode 100644
index 00000000..cebccec1
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameOrientation.java
new file mode 100644
index 00000000..fef4624a
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint2D.java
new file mode 100644
index 00000000..0a5575d1
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePoint3D.java
new file mode 100644
index 00000000..5b7ad253
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFramePose3D.java
new file mode 100644
index 00000000..f27cdf31
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameQuaternion.java
new file mode 100644
index 00000000..7cbcebea
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector2D.java
new file mode 100644
index 00000000..da92c1d8
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoFrameVector3D.java
new file mode 100644
index 00000000..64760b2e
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RateLimitedYoMutableFrameVector3D.java
new file mode 100644
index 00000000..8e68b73a
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/RunningAverageYoFrameVector3D.java
new file mode 100644
index 00000000..556f6bd5
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java
new file mode 100644
index 00000000..51ae84ab
--- /dev/null
+++ b/src/filters/java/us/ihmc/yoVariables/euclid/filters/SecondOrderFilteredYoFrameVector3D.java
@@ -0,0 +1,128 @@
+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.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,
+ SecondOrderFilteredYoDouble.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,
+ SecondOrderFilteredYoDouble.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/src/filters/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java b/src/filters/java/us/ihmc/yoVariables/euclid/filters/SimpleMovingAverageFilteredYoFrameVector3D.java
new file mode 100644
index 00000000..07e32c51
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java
new file mode 100644
index 00000000..ee5817bc
--- /dev/null
+++ b/src/filters/java/us/ihmc/yoVariables/filters/AccelerationLimitedYoVariable.java
@@ -0,0 +1,141 @@
+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;
+
+/**
+ * Creates a yo variable
+ */
+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/src/filters/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaBasedOnBreakFrequencyProvider.java
new file mode 100644
index 00000000..836eeb91
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java
new file mode 100644
index 00000000..6329dbe1
--- /dev/null
+++ b/src/filters/java/us/ihmc/yoVariables/filters/AlphaBetaFilteredYoVariable.java
@@ -0,0 +1,96 @@
+package us.ihmc.yoVariables.filters;
+
+import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoDouble;
+
+/**
+ * + * 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 + * + *
+ *+ * + * 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, + YoRegistry 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 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/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java new file mode 100644 index 00000000..6f09dbe3 --- /dev/null +++ b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilterTools.java @@ -0,0 +1,40 @@ +package us.ihmc.yoVariables.filters; + +import us.ihmc.commons.AngleTools; +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}. + * + *
+ * 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 + */ + 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/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredWrappingYoVariable.java new file mode 100644 index 00000000..60a1bd07 --- /dev/null +++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java new file mode 100644 index 00000000..47580e57 --- /dev/null +++ b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java @@ -0,0 +1,88 @@ +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, 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, 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/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java new file mode 100644 index 00000000..0a0bfa32 --- /dev/null +++ b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoVariable.java @@ -0,0 +1,173 @@ +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; +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/src/filters/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/BacklashCompensatingVelocityYoVariable.java
new file mode 100644
index 00000000..768fcabe
--- /dev/null
+++ b/src/filters/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+ * 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/src/filters/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java
new file mode 100644
index 00000000..0ec1cf4a
--- /dev/null
+++ b/src/filters/java/us/ihmc/yoVariables/filters/DeadbandedYoVariable.java
@@ -0,0 +1,60 @@
+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;
+
+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/src/filters/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java b/src/filters/java/us/ihmc/yoVariables/filters/DelayedYoBoolean.java
new file mode 100644
index 00000000..3d18c14f
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/DelayedYoDouble.java
new file mode 100644
index 00000000..47301666
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/DeltaLimitedYoVariable.java
new file mode 100644
index 00000000..688363d1
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java
new file mode 100644
index 00000000..e548ae57
--- /dev/null
+++ b/src/filters/java/us/ihmc/yoVariables/filters/FilteredDiscreteVelocityYoVariable.java
@@ -0,0 +1,163 @@
+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+ * 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 = EuclidCoreTools.angleDifferenceMinusPiToPi(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/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
new file mode 100644
index 00000000..3599edc7
--- /dev/null
+++ b/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderBandPassFilteredYoDouble.java
@@ -0,0 +1,70 @@
+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 FirstOrderFilteredYoDouble
+{
+ private boolean hasBeenCalled = false;
+
+ private final FirstOrderFilteredYoDouble highPassFilteredInput;
+
+ public FirstOrderBandPassFilteredYoDouble(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz,
+ YoDouble yoTime, YoRegistry registry)
+ {
+ super(name, description, maxPassThroughFrequency_Hz, yoTime, FirstOrderFilteredYoDouble.FirstOrderFilterType.LOW_PASS, registry);
+
+ this.highPassFilteredInput = new FirstOrderFilteredYoDouble(name + "HighPassFilteredOnly", description, minPassThroughFrequency_Hz, yoTime,
+ FirstOrderFilteredYoDouble.FirstOrderFilterType.HIGH_PASS, registry);
+
+ setPassBand(minPassThroughFrequency_Hz, maxPassThroughFrequency_Hz);
+ }
+
+ public FirstOrderBandPassFilteredYoDouble(String name, String description, double minPassThroughFrequency_Hz, double maxPassThroughFrequency_Hz,
+ double DT, YoRegistry registry)
+ {
+ super(name, description, maxPassThroughFrequency_Hz, DT, FirstOrderFilteredYoDouble.FirstOrderFilterType.LOW_PASS, registry);
+
+ 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
+ + " ]");
+ }
+ }
+
+ 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/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
new file mode 100644
index 00000000..71e28fd3
--- /dev/null
+++ b/src/filters/java/us/ihmc/yoVariables/filters/FirstOrderFilteredYoDouble.java
@@ -0,0 +1,163 @@
+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, HIGH_PASS
+ }
+
+ private boolean hasBeenCalled = false;
+
+ private double filterInputOld;
+ private double filterUpdateTimeOld;
+
+ private final YoDouble cutoffFrequencyHz;
+
+ private final DoubleProvider yoTime;
+ private double dt;
+
+ private final 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 = computeAlpha(dt, cutoffFrequencyHz.getDoubleValue());
+
+ return alpha * this.getDoubleValue() + (1.0 - alpha) * filterInput;
+ }
+
+ private double computeHighPassUpdate(double filterInput, double dt)
+ {
+ double alpha = computeAlpha(dt, cutoffFrequencyHz.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.cutoffFrequencyHz.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.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);
+ }
+
+ filterInputOld = filterInput;
+
+ if (yoTime != null)
+ {
+ filterUpdateTimeOld = yoTime.getValue();
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/filters/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java b/src/filters/java/us/ihmc/yoVariables/filters/GlitchFilteredYoBoolean.java
new file mode 100644
index 00000000..c2f6d2f0
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java b/src/filters/java/us/ihmc/yoVariables/filters/GlitchFilteredYoInteger.java
new file mode 100644
index 00000000..469e7a70
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/JerkLimitedYoDouble.java
new file mode 100644
index 00000000..8f66d063
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java b/src/filters/java/us/ihmc/yoVariables/filters/MovingAverageYoDouble.java
new file mode 100644
index 00000000..bad8f2db
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java
new file mode 100644
index 00000000..b38e0214
--- /dev/null
+++ b/src/filters/java/us/ihmc/yoVariables/filters/ProcessingYoVariable.java
@@ -0,0 +1,10 @@
+package us.ihmc.yoVariables.filters;
+
+public interface ProcessingYoVariable
+{
+ void update();
+
+ default void reset()
+ {
+ }
+}
diff --git a/src/filters/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java
new file mode 100644
index 00000000..475a49ab
--- /dev/null
+++ b/src/filters/java/us/ihmc/yoVariables/filters/RateLimitedYoVariable.java
@@ -0,0 +1,217 @@
+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;
+
+/**
+ * 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;
+
+ private final DoubleProvider unlimitedPosition;
+ private final YoBoolean limited;
+
+ private final double dt;
+
+ 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); + + this.hasBeenCalled = VariableTools.createHasBeenCalledYoBoolean(name, "", registry); + this.limited = VariableTools.createLimitedCalledYoBoolean(name, "", registry); + + this.unlimitedPosition = unlimitedPosition; + this.maxRateVariable = maxRateVariable; + + this.dt = dt; + + 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: + *+ *
+ *
+ * ---------------------------------------------------------------- + * 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;
+ }
+
+ public enum SecondOrderFilterType
+ {
+ LOW_PASS, NOTCH, BAND, HIGH_PASS
+ }
+}
diff --git a/src/filters/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java b/src/filters/java/us/ihmc/yoVariables/filters/SecondOrderFilteredYoVariableParameters.java
new file mode 100644
index 00000000..b7ce88ad
--- /dev/null
+++ b/src/filters/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 SecondOrderFilteredYoDouble.SecondOrderFilterType filterType;
+
+ public SecondOrderFilteredYoVariableParameters(String name, YoRegistry registry, double naturalFrequencyInHz, double dampingRatio,
+ SecondOrderFilteredYoDouble.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 SecondOrderFilteredYoDouble.SecondOrderFilterType getFilterType()
+ {
+ return filterType;
+ }
+}
diff --git a/src/filters/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java b/src/filters/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
new file mode 100644
index 00000000..b6a0b8f0
--- /dev/null
+++ b/src/filters/java/us/ihmc/yoVariables/filters/SimpleMovingAverageFilteredYoVariable.java
@@ -0,0 +1,88 @@
+package us.ihmc.yoVariables.filters;
+
+import org.ejml.data.DMatrixRMaj;
+
+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!
+ */
+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);
+ Arrays.fill(previousUpdateValues.data, 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/src/filters/java/us/ihmc/yoVariables/filters/VariableTools.java b/src/filters/java/us/ihmc/yoVariables/filters/VariableTools.java
new file mode 100644
index 00000000..d798ac1a
--- /dev/null
+++ b/src/filters/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/src/filters/java/us/ihmc/yoVariables/filters/YoMatrix.java b/src/filters/java/us/ihmc/yoVariables/filters/YoMatrix.java
new file mode 100644
index 00000000..e640d87b
--- /dev/null
+++ b/src/filters/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