Some more minor cleanup, also moved the TrajectoryGenerator to the example.

SylvainBertrand · SylvainBertrand · commit c6fab68ad106 · 2024-06-14T10:04:16.000-05:00
diff --git a/example-simulations/src/main/java/us/ihmc/exampleSimulations/yoFilteredDouble/emptyRobotSCS/YoFilteredDoubleController.java b/example-simulations/src/main/java/us/ihmc/exampleSimulations/yoFilteredDouble/emptyRobotSCS/YoFilteredDoubleController.java
@@ -1,15 +1,16 @@
 package us.ihmc.exampleSimulations.yoFilteredDouble.emptyRobotSCS;
 
+import us.ihmc.exampleSimulations.yoFilteredDouble.emptyRobotSCS.YoFilteredDoubleController.TrajectoryGenerator.ChirpType;
+import us.ihmc.exampleSimulations.yoFilteredDouble.emptyRobotSCS.YoFilteredDoubleController.TrajectoryGenerator.Trajectory;
 import us.ihmc.robotics.math.filters.ContinuousTransferFunction;
 import us.ihmc.robotics.math.filters.TransferFunctionDiscretizer;
 import us.ihmc.robotics.math.filters.YoFilteredDouble;
-import us.ihmc.robotics.math.trajectories.generators.TrajectoryGenerator;
-import us.ihmc.robotics.math.trajectories.generators.TrajectoryGenerator.ChirpType;
-import us.ihmc.robotics.math.trajectories.generators.TrajectoryGenerator.Trajectory;
 import us.ihmc.simulationconstructionset.util.RobotController;
 import us.ihmc.yoVariables.registry.YoRegistry;
 import us.ihmc.yoVariables.variable.YoDouble;
 
+import java.util.ArrayList;
+
 public class YoFilteredDoubleController implements RobotController
 {
    private YoRegistry registry;
@@ -195,4 +196,235 @@ public void doControl()
       RefTraj2_nonjump_Filtered_Var.set(sinusoid_input.getDoubleValue());
       chirpFreqHz.set(chirp.getFreqCheckRad() / (2 * Math.PI));
    }
+
+   /**
+    * Class is designed for quickly generating several different trajectory types. A majority of the classes are self-explanatory.
+    * The Triangle Waveform comes from https://en.wikipedia.org/wiki/Triangle_wave, denoting an alternative equation because the mod
+    * operator is not the remainder operator.
+    * The Multi-Square Waveform will generate a series of square waves in order repeating forever. It can be initialized using:
+    * DesiredTrajectory = new TrajectoryGenerator("OpenLoop", registry, Trajectory.MULTI_SQUARE, 2000.0, 0.2, new ArrayList<Double>(Arrays.asList(1000.0, 0.0,
+    * 2000.0, 0.0, 3000.0, 0.0)));
+    * The Chirp signal is a sinusoid with a consistent amplitude, but is increasing in frequency. It is commonly used for System ID,
+    * and was used for identifying the transfer function of the actuator. The equation was given by Dr. Southward from VT in the ME Dept.
+    * There are two options for chirp signals: linear or exponential. This refers to the rate at which the frequency is increasing and
+    * this choice depends on which frequencies you care about.
+    *
+    * @author Connor Herron
+    */
+
+   public static class TrajectoryGenerator
+   {
+      private String name;
+      private double totalTime;
+      private double minFreqRad;
+      private double maxFreqRad;
+      private double prevCos;
+      private double prevSin;
+      private double chirpSlope;
+      private double chirpAmplitude;
+      private double curFreqRad;
+      private double expoCoeff;
+      private YoDouble freqCheckRad;
+      private YoRegistry registry;
+      private ArrayList<Double> multiSquareList = new ArrayList<>();
+
+      private double waveAmp;
+      private double waveFreq;
+      private double rampSlope;
+      private double phaseOffset;
+      private double amplitudeOffset;
+      private double wavePeriod;
+
+      private int initFlag = 0;
+
+      public enum Trajectory
+      {
+         STEP, RAMP, SINE, TRIANGLE, SAWTOOTH, SQUARE, MULTI_SQUARE, CHIRP
+      }
+
+      public enum ChirpType
+      {
+         LINEAR, EXPONENTIAL
+      }
+
+      private Trajectory myTrajectory;
+      private ChirpType myChirpType;
+
+      public TrajectoryGenerator(String name, YoRegistry registry, Trajectory desiredWaveform, double waveAmp)
+      {
+         this(name, registry, desiredWaveform, waveAmp, 0.0);
+      }
+
+      public TrajectoryGenerator(String name, YoRegistry registry, Trajectory desiredWaveform, double waveAmp, double waveFreq)
+      {
+         this(name, registry, desiredWaveform, waveAmp, waveFreq, 0.0);
+      }
+
+      public TrajectoryGenerator(String name, YoRegistry registry, Trajectory desiredWaveform, double waveAmp, double waveFreq, double rampSlope)
+      {
+         this(name, registry, desiredWaveform, waveAmp, waveFreq, rampSlope, 0.0, 0.0);
+      }
+
+      public TrajectoryGenerator(String name,
+                                 YoRegistry registry,
+                                 Trajectory desiredWaveform,
+                                 double waveAmp,
+                                 double waveFreq,
+                                 double rampSlope,
+                                 double phaseOffset,
+                                 double amplitudeOffset)
+      {
+         this.name = name;
+         this.registry = registry;
+         this.myTrajectory = desiredWaveform;
+         this.waveAmp = waveAmp;
+         this.waveFreq = waveFreq;
+         this.wavePeriod = 1 / waveFreq;
+         this.rampSlope = rampSlope;
+         this.phaseOffset = phaseOffset;
+         this.amplitudeOffset = amplitudeOffset;
+      }
+
+      public TrajectoryGenerator(String name, YoRegistry registry, Trajectory desiredWaveform, double waveAmp, double waveFreq, ArrayList<Double> multiSquare)
+      {
+         this.name = name;
+         this.registry = registry;
+         this.myTrajectory = desiredWaveform;
+         this.waveAmp = waveAmp;
+         this.waveFreq = waveFreq;
+         this.wavePeriod = wavePeriod;
+         this.multiSquareList = multiSquare;
+      }
+
+      // For running chirp signals
+      public TrajectoryGenerator(String name,
+                                 YoRegistry registry,
+                                 Trajectory desiredWaveform,
+                                 ChirpType desiredChirpType,
+                                 double totalTime,
+                                 double amplitude,
+                                 double minFreqHz,
+                                 double maxFreqHz)
+      {
+         this.name = name;
+         this.registry = registry;
+         this.myTrajectory = desiredWaveform;
+         this.myChirpType = desiredChirpType;
+
+         freqCheckRad = new YoDouble(name + "freqCheckRad", registry);
+
+         initializeChirpSignal(totalTime, amplitude, minFreqHz, maxFreqHz);
+      }
+
+      public void initializeChirpSignal(double totalTime, double amplitude, double minFreqHz, double maxFreqHz)
+      {
+         this.totalTime = totalTime;
+         this.minFreqRad = 2 * Math.PI * minFreqHz;
+         this.maxFreqRad = 2 * Math.PI * maxFreqHz;
+         this.chirpAmplitude = amplitude;
+
+         // Calculate the slope of the chirp signal.
+         this.chirpSlope = (this.maxFreqRad - this.minFreqRad) / (totalTime);
+
+         // Initialize coefficient for the exponential function
+         this.expoCoeff = Math.log10(maxFreqRad / minFreqRad) / totalTime;
+
+         // initialize prevCos and prevSin
+         this.prevCos = 1.0;
+         this.prevSin = 0.0;
+      }
+
+      private double alpha;
+      private double beta;
+      private double cosNew;
+      private double sinNew;
+
+      /**
+       * Method calculates the open loop current command (mA) to send for a linear sinusoidal chirp signal. This method is initialized
+       * with the initializeOLChirpSignal method above.
+       *
+       * @param t_sec real time seconds.
+       * @param dT    real time control dT
+       * @return commanded motor current (mA)
+       */
+      public double runChirpSignal(double t_sec, double dT)
+      {
+         // Calculate current Freq (Rad)
+         switch (myChirpType)
+         {
+            case LINEAR:
+               this.curFreqRad = (this.chirpSlope * t_sec + this.minFreqRad);
+               break;
+            case EXPONENTIAL:
+               this.curFreqRad = this.minFreqRad * Math.pow(10, this.expoCoeff * t_sec);
+               break;
+         }
+
+         // Calculate new sinusoid value using double angle identity formula.
+         alpha = Math.cos(this.curFreqRad * dT);
+         beta = Math.sin(this.curFreqRad * dT);
+
+         cosNew = alpha * this.prevCos - beta * this.prevSin;
+         sinNew = beta * this.prevCos + alpha * this.prevSin;
+
+         this.prevCos = cosNew;
+         this.prevSin = sinNew;
+
+         if (t_sec > this.totalTime)
+         {
+            freqCheckRad.set(0.0);
+            return 0.0;
+         }
+         else
+         {
+            freqCheckRad.set(this.curFreqRad);
+            return this.chirpAmplitude * sinNew;
+         }
+      }
+
+      // Run method during loop.
+      public double updateTrajectory(double curSecTime, double dT)
+      {
+         switch (myTrajectory)
+         {
+            case STEP:
+               return waveAmp;
+            case RAMP:
+               return rampSlope * curSecTime;
+            case SINE:
+               return waveAmp * Math.sin(2 * Math.PI * waveFreq * curSecTime + phaseOffset) + amplitudeOffset;
+            case TRIANGLE:
+               return (4 * waveAmp / wavePeriod) * Math.abs((((curSecTime - (wavePeriod / 4)) % wavePeriod) + wavePeriod) % wavePeriod - wavePeriod / 2)
+                      - waveAmp;
+            case SAWTOOTH:
+               return waveAmp * waveFreq * Math.abs(curSecTime % (1 / waveFreq));
+            case SQUARE:
+               return waveAmp * Math.signum(Math.sin(2 * Math.PI * waveFreq * curSecTime + phaseOffset)) + amplitudeOffset;
+            case MULTI_SQUARE:
+               return waveAmp = multiSquareList.get(Math.floorMod((int) (curSecTime * waveFreq), multiSquareList.size()));
+            case CHIRP:
+               return runChirpSignal(curSecTime, dT);
+            default:
+               return 0.0;
+         }
+      }
+
+      // Wave Frequency is defined as [rad/s]
+      public double updateTrajectoryDot(Trajectory Desired_Trajectory, double curSecTime, double waveAmp, double waveFreq, double phaseOffset)
+      {
+         if (Desired_Trajectory == Trajectory.SINE)
+         {
+            return 2 * Math.PI * waveFreq * waveAmp * Math.cos(2 * Math.PI * waveFreq * curSecTime + phaseOffset);
+         }
+         else
+         {
+            return 0;
+         }
+      }
+
+      public double getFreqCheckRad()
+      {
+         return freqCheckRad.getDoubleValue();
+      }
+   }
 }
diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ContinuousTransferFunction.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/ContinuousTransferFunction.java
@@ -34,12 +34,12 @@ public ContinuousTransferFunction()
     * @param numerator   - numerator coefficients in power order (see above).
     * @param denominator - denominator coefficients in power order (see above).
     */
-   public ContinuousTransferFunction(double k, double numerator[], double denominator[])
+   public ContinuousTransferFunction(double k, double[] numerator, double[] denominator)
    {
       this("", k, numerator, denominator);
    }
 
-   public ContinuousTransferFunction(String name, double k, double numerator[], double denominator[])
+   public ContinuousTransferFunction(String name, double k, double[] numerator, double[] denominator)
    {
       this.name = name;
       this.k = k;
@@ -56,13 +56,13 @@ public ContinuousTransferFunction(String name, ContinuousTransferFunction[] seve
    {
       this.name = name;
 
-      int numOfTranferFunctionsToCascade = severalContinuousTransferFunctions.length;
+      int numOfTransferFunctionsToCascade = severalContinuousTransferFunctions.length;
       ContinuousTransferFunction tf_total;
 
       // Initialize total transfer function with first transfer function.
       tf_total = severalContinuousTransferFunctions[0];
 
-      for (int i = 1; i < numOfTranferFunctionsToCascade; i++)
+      for (int i = 1; i < numOfTransferFunctionsToCascade; i++)
       {
          tf_total = CascadeTwoTransferFunctions(tf_total, severalContinuousTransferFunctions[i]);
       }
@@ -75,10 +75,11 @@ public ContinuousTransferFunction(String name, ContinuousTransferFunction[] seve
 
    /**
     * Method cascades two transfer functions, H1(s) and H2(s) and returns a new transfer function H3(s).
-    *
-    * N1(s)      N2(s)        N3(s)
+    * <pre>
+    *                            N1(s)      N2(s)        N3(s)
     * H3(s) = H1(s)*H2(s) = k1 * ----- k2 * ----- = k3 * -----
-    * D1(s)      D2(s)        D3(s)
+    *                            D1(s)      D2(s)        D3(s)
+    * </pre>
     *
     * @param H1 - Transfer Function 1
     * @param H2 - Transfer Function 2
@@ -89,8 +90,8 @@ private ContinuousTransferFunction CascadeTwoTransferFunctions(ContinuousTransfe
       ContinuousTransferFunction H3 = new ContinuousTransferFunction();
 
       H3.setGain(H1.getGain() * H2.getGain());
-      H3.setNumerator(CascadePolynomials(H1.getNumerator(), H2.getNumerator()));
-      H3.setDenominator(CascadePolynomials(H1.getDenominator(), H2.getDenominator()));
+      H3.setNumerator(cascadePolynomials(H1.getNumerator(), H2.getNumerator()));
+      H3.setDenominator(cascadePolynomials(H1.getDenominator(), H2.getDenominator()));
 
       return H3;
    }
@@ -102,7 +103,7 @@ private ContinuousTransferFunction CascadeTwoTransferFunctions(ContinuousTransfe
     * @param poly2
     * @return poly3 = poly1*poly2
     */
-   private double[] CascadePolynomials(double[] poly1, double[] poly2)
+   private static double[] cascadePolynomials(double[] poly1, double[] poly2)
    {
       // Initialize new polynomial to new size.
       double[] poly3 = new double[poly1.length + poly2.length - 1];
diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/YoFilteredDouble.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/YoFilteredDouble.java
@@ -22,21 +22,20 @@
 public class YoFilteredDouble
 {
 
-   private TransferFunctionDiscretizer filter;
+   private final TransferFunctionDiscretizer filter;
 
-   private DMatrixRMaj inputCoefficients;
-   private DMatrixRMaj outputCoefficients;
+   private final DMatrixRMaj inputCoefficients;
+   private final DMatrixRMaj outputCoefficients;
 
-   private DMatrixRMaj inputHistory;
-   private DMatrixRMaj outputHistory;
+   private final DMatrixRMaj inputHistory;
+   private final DMatrixRMaj outputHistory;
 
    private final YoDouble filteredYoDouble;
    private final YoDouble rawYoDouble;
    private boolean firstTick = true;
-   private double[] inTemp;
-   private double[] outTemp;
-   private double newFilteredValue;
-   private boolean safeStartup;
+   private final double[] inTemp;
+   private final double[] outTemp;
+   private final boolean safeStartup;
 
    public YoFilteredDouble(YoRegistry registry, TransferFunctionDiscretizer filter)
    {
@@ -49,11 +48,11 @@ public YoFilteredDouble(String name, YoRegistry registry, TransferFunctionDiscre
    }
 
    /**
-    * @param name        - class name
-    * @param registry    - YoRegistry to add YoVariables to.
-    * @param filter      - Filter object from trec-simulation-tools.YoVariables.
-    * @param safeStartup -	Recommended to set true. Boolean refers to whether you would like to set the
-    *                    to avoid input/output histories of zero. Without this enabled, you will start the first ticks with high outputs.
+    * @param name        class name
+    * @param registry    YoRegistry to add YoVariables to.
+    * @param filter      Filter object from trec-simulation-tools.YoVariables.
+    * @param safeStartup Recommended to set true. Boolean refers to whether you would like to set the to avoid input/output histories of zero.
+    *                    Without this enabled, you will start the first ticks with high outputs.
     */
    public YoFilteredDouble(String name, YoRegistry registry, TransferFunctionDiscretizer filter, boolean safeStartup)
    {
@@ -127,7 +126,7 @@ public void set(double newRawValue)
       inputHistory.set(1, inTemp.length, false, inTemp);
 
       // Solve for new filtered value.
-      newFilteredValue = CommonOps_DDRM.dot(inputCoefficients, inputHistory) + CommonOps_DDRM.dot(outputCoefficients, outputHistory);
+      double newFilteredValue = CommonOps_DDRM.dot(inputCoefficients, inputHistory) + CommonOps_DDRM.dot(outputCoefficients, outputHistory);
 
       // Set new filtered value.
       filteredYoDouble.set(newFilteredValue);
diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/TrajectoryGenerator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/trajectories/generators/TrajectoryGenerator.java
