Feature/add filters

build.gradle.kts

@@ -23,7 +23,15 @@ dependencies {
    api("us.ihmc:euclid-frame:0.21.0")
 }
 
+filtersDependencies {
+   api(ihmc.sourceSetProject("main"))
+   api("org.ejml:ejml-ddense:0.39")
+//   api("us.ihmc:ihmc-commons-utils:source")
+}
+
 testDependencies {
+   api(ihmc.sourceSetProject("filters"))
    api("us.ihmc:ihmc-commons-testing:0.32.0")
    api("us.ihmc:euclid-test:0.21.0")
+   api("org.apache.commons:commons-math3:3.3")
 }

gradle.properties

@@ -1,4 +1,4 @@
 title = IHMC YoVariables
-extraSourceSets = ["test"]
+extraSourceSets = ["test", "filters"]
 compositeSearchHeight = 0
 excludeFromCompositeBuild = false

src/filters/java/us/ihmc/yoVariables/euclid/filters/AccelerationLimitedYoFrameVector3D.java

@@ -0,0 +1,160 @@
+package us.ihmc.yoVariables.euclid.filters;
+
+import us.ihmc.euclid.referenceFrame.FrameVector3D;
+import us.ihmc.euclid.referenceFrame.ReferenceFrame;
+import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly;
+import us.ihmc.yoVariables.euclid.referenceFrame.YoFrameVector3D;
+import us.ihmc.yoVariables.filters.VariableTools;
+import us.ihmc.yoVariables.providers.DoubleProvider;
+import us.ihmc.yoVariables.registry.YoRegistry;
+import us.ihmc.yoVariables.variable.YoBoolean;
+import us.ihmc.yoVariables.variable.YoDouble;
+
+public class AccelerationLimitedYoFrameVector3D extends YoFrameVector3D
+{
+   private final DoubleProvider maxRateVariable;
+   private final DoubleProvider maxAccelerationVariable;
+
+   private final FrameTuple3DReadOnly rawPosition;
+   private final YoBoolean accelerationLimited;
+   private final YoBoolean rateLimited;
+
+   private final YoBoolean hasBeenInitialized;
+   private final YoDouble positionGain;
+   private final YoDouble velocityGain;
+
+   private final YoFrameVector3D smoothedRate;
+   private final YoFrameVector3D smoothedAcceleration;
+
+   private final double dt;
+
+   private final FrameVector3D positionError;
+   private final FrameVector3D acceleration;
+
+   public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate,
+                                             DoubleProvider maxAcceleration, double dt, FrameTuple3DReadOnly rawPosition)
+   {
+      this(namePrefix, nameSuffix, registry, maxRate, maxAcceleration, dt, rawPosition, rawPosition.getReferenceFrame());
+   }
+
+   public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate,
+                                             DoubleProvider maxAcceleration, double dt, ReferenceFrame referenceFrame)
+   {
+      this(namePrefix, nameSuffix, registry, maxRate, maxAcceleration, dt, null, referenceFrame);
+   }
+   public AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, double maxRate, double maxAcceleration,
+                                             double dt, ReferenceFrame referenceFrame)
+   {
+      this(namePrefix, nameSuffix, registry, VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, maxRate, registry),
+           VariableTools.createMaxAccelerationYoDouble(namePrefix, nameSuffix, maxAcceleration, registry), dt, null, referenceFrame);
+   }
+
+   private AccelerationLimitedYoFrameVector3D(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maxRate,
+                                              DoubleProvider maxAcceleration, double dt, FrameTuple3DReadOnly rawPosition, ReferenceFrame referenceFrame)
+   {
+      super(namePrefix, nameSuffix, referenceFrame, registry);
+
+      positionError = new FrameVector3D(referenceFrame);
+      acceleration = new FrameVector3D(referenceFrame);
+
+      if (maxRate == null)
+         maxRate = VariableTools.createMaxRateYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry);
+      if (maxAcceleration == null)
+         maxAcceleration = VariableTools.createMaxAccelerationYoDouble(namePrefix, nameSuffix, Double.POSITIVE_INFINITY, registry);
+
+      this.maxRateVariable = maxRate;
+      this.maxAccelerationVariable = maxAcceleration;
+
+      this.dt = dt;
+
+      hasBeenInitialized = new YoBoolean(namePrefix + "HasBeenInitialized" + namePrefix, registry);
+      this.rateLimited = new YoBoolean(namePrefix + "RateLimited" + nameSuffix, registry);
+      this.accelerationLimited = new YoBoolean(namePrefix + "AccelerationLimited" + nameSuffix, registry);
+
+      smoothedRate = new YoFrameVector3D(namePrefix + "SmoothedRate" + namePrefix, referenceFrame, registry);
+      smoothedAcceleration = new YoFrameVector3D(namePrefix + "SmoothedAcceleration" + namePrefix, referenceFrame, registry);
+
+      positionGain = new YoDouble(namePrefix + "PositionGain" + namePrefix, registry);
+      velocityGain = new YoDouble(namePrefix + "VelocityGain" + namePrefix, registry);
+
+      double w0 = 2.0 * Math.PI * 16.0;
+      double zeta = 1.0;
+
+      setGainsByPolePlacement(w0, zeta);
+      hasBeenInitialized.set(false);
+
+      this.rawPosition = rawPosition;
+
+   }
+
+
+   public void setGainsByPolePlacement(double w0, double zeta)
+   {
+      positionGain.set(w0 * w0);
+      velocityGain.set(2.0 * zeta * w0);
+   }
+
+   public void update()
+   {
+      if (rawPosition == null)
+      {
+         throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null "
+                                              + "position variable to call update(), otherwise use update(double)");
+      }
+
+      update(rawPosition);
+   }
+
+   public void update(FrameTuple3DReadOnly frameVectorUnfiltered)
+   {
+      checkReferenceFrameMatch(frameVectorUnfiltered);
+      update(frameVectorUnfiltered.getX(), frameVectorUnfiltered.getY(), frameVectorUnfiltered.getZ());
+   }
+
+   public void update(double xUnfiltered, double yUnfiltered, double zUnfiltered)
+   {
+      if (!hasBeenInitialized.getBooleanValue())
+         initialize(xUnfiltered, yUnfiltered, zUnfiltered);
+
+      positionError.set(xUnfiltered, yUnfiltered, zUnfiltered);
+      positionError.sub(this);
+
+      acceleration.set(smoothedRate);
+      acceleration.scale(-velocityGain.getValue());
+      acceleration.scaleAdd(positionGain.getValue(), positionError, acceleration);
+
+      accelerationLimited.set(acceleration.clipToMaxLength(maxAccelerationVariable.getValue()));
+
+      smoothedAcceleration.set(acceleration);
+      smoothedRate.scaleAdd(dt, smoothedAcceleration, smoothedRate);
+
+      rateLimited.set(smoothedRate.clipToMaxLength(maxRateVariable.getValue()));
+
+      this.scaleAdd(dt, smoothedRate, this);
+
+      if (this.containsNaN())
+         throw new RuntimeException("what?");
+
+   }
+
+   public void initialize(FrameTuple3DReadOnly input)
+   {
+      initialize(input.getX(), input.getY(), input.getZ());
+   }
+
+   public void initialize(double xInput, double yInput, double zInput)
+   {
+      this.set(xInput, yInput, zInput);
+      smoothedRate.setToZero();
+      smoothedAcceleration.setToZero();
+
+      this.hasBeenInitialized.set(true);
+   }
+
+   public void reset()
+   {
+      this.hasBeenInitialized.set(false);
+      smoothedRate.setToZero();
+      smoothedAcceleration.setToZero();
+   }
+}

src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredRigidBodyTransform.java

@@ -0,0 +1,43 @@
+package us.ihmc.yoVariables.euclid.filters;
+
+import us.ihmc.euclid.transform.RigidBodyTransform;
+
+public class AlphaFilteredRigidBodyTransform extends RigidBodyTransform
+{
+   // an alpha of zero applies zero filtering, accepting the entirely new input.
+   private double alpha = 0.0;
+   private final RigidBodyTransform previousFiltered = new RigidBodyTransform();
+
+   public AlphaFilteredRigidBodyTransform()
+   {
+      reset();
+   }
+
+   public void update(RigidBodyTransform measured)
+   {
+      if (containsNaN())
+      {
+         set(measured);
+      }
+      else
+      {
+         previousFiltered.set(this);
+         interpolate(measured, previousFiltered, alpha);
+      }
+   }
+
+   public double getAlpha()
+   {
+      return alpha;
+   }
+
+   public void setAlpha(double alpha)
+   {
+      this.alpha = alpha;
+   }
+
+   public void reset()
+   {
+      setToNaN();
+   }
+}

src/filters/java/us/ihmc/yoVariables/euclid/filters/AlphaFilteredTuple2D.java

@@ -0,0 +1,108 @@
+package us.ihmc.yoVariables.euclid.filters;
+
+import org.apache.commons.lang3.NotImplementedException;
+
+import us.ihmc.euclid.interfaces.EuclidGeometry;
+import us.ihmc.euclid.tools.EuclidCoreTools;
+import us.ihmc.euclid.transform.interfaces.Transform;
+import us.ihmc.euclid.tuple2D.interfaces.Tuple2DBasics;
+import us.ihmc.euclid.tuple2D.interfaces.Tuple2DReadOnly;
+import us.ihmc.yoVariables.providers.DoubleProvider;
+
+public class AlphaFilteredTuple2D implements Tuple2DBasics
+{
+   private final DoubleProvider alpha;
+
+   private boolean resetX;
+   private boolean resetY;
+
+   private double x;
+   private double y;
+
+   public AlphaFilteredTuple2D(DoubleProvider alpha)
+   {
+      this.alpha = alpha;
+      reset();
+   }
+
+   public AlphaFilteredTuple2D(Tuple2DReadOnly initialValue, DoubleProvider alpha)
+   {
+      this.alpha = alpha;
+      reset(initialValue);
+   }
+
+   public void reset()
+   {
+      resetX = true;
+      resetY = true;
+   }
+
+   public void reset(Tuple2DReadOnly other)
+   {
+      reset(other.getX(), other.getY());
+   }
+
+   public void reset(double x, double y)
+   {
+      reset();
+      set(x, y);
+   }
+
+   @Override
+   public double getX()
+   {
+      return x;
+   }
+
+   @Override
+   public double getY()
+   {
+      return y;
+   }
+
+   @Override
+   public void setX(double x)
+   {
+      if (resetX)
+      {
+         this.x = x;
+         resetX = false;
+      }
+      else
+      {
+         this.x = EuclidCoreTools.interpolate(x, this.x, alpha.getValue());
+      }
+   }
+
+   @Override
+   public void setY(double y)
+   {
+      if (resetY)
+      {
+         this.y = y;
+         resetY = false;
+      }
+      else
+      {
+         this.y = EuclidCoreTools.interpolate(y, this.y, alpha.getValue());
+      }
+   }
+
+   @Override
+   public boolean geometricallyEquals(EuclidGeometry geometry, double epsilon)
+   {
+      return epsilonEquals(geometry, epsilon);
+   }
+
+   @Override
+   public void applyTransform(Transform transform, boolean checkIfTransformInXYPlane)
+   {
+      throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + ".");
+   }
+
+   @Override
+   public void applyInverseTransform(Transform transform, boolean checkIfTransformInXYPlane)
+   {
+      throw new NotImplementedException("Not supported by " + getClass().getSimpleName() + ".");
+   }
+}