diff --git a/README.md b/README.md index fff5d9ea..2b79d6a3 100644 --- a/README.md +++ b/README.md @@ -40,6 +40,13 @@ A `YoParameter` can be attributed an initial value at construction or can also b ### `YoBuffer` `YoBuffer` provides an implementation of a buffer which can be used to store value history of collection of `YoVariable`s. +### `Filters` +The `Filters` project provides a way of generating yo variables with diffeerent types of filters on the resulting output signal. This includes: +- `AlphaFilteredYoVariable`, which applies an alpha filter to a `YoDouble`, which is equivalent to a low-pass filter. +- `RateLimitedYoVariable`, which limits the output of a `YoDouble` to change less than a cerrtain rate. +- `BacklashCompensatedVelocityYoVariable`, which attempts to remove the backlash from the velocity that is estimated from a finite-differenced position signal. +- `GlitchFilteredYoBoolean`, which forces a system to change value for a certain number of times before the return changes value. + ## Using YoVariables from .jar releases with Maven/Gradle The releases .jars for YoVariables are hosted on Bintray. You can browse the IHMC release packages at https://bintray.com/ihmcrobotics/maven-release. @@ -53,8 +60,7 @@ repositories { } dependencies { - compile group: "us.ihmc", name: "ihmc-yovariables", version: "x.x" + compile group: "us.ihmc", name: "ihmc-yovariables", version: "x.x", + compile group: "us.ihmc", name: "ihmc-yovariables-filters", version: "x.x" } ``` - - diff --git a/build.gradle.kts b/build.gradle.kts index 2c095026..f30c48ad 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -6,10 +6,10 @@ plugins { ihmc { group = "us.ihmc" - version = "0.12.2" + version = "0.13.1" vcsUrl = "https://github.com/ihmcrobotics/ihmc-yovariables" openSource = true - maintainer = "Sylvain Bertrand (sbertrand@ihmc.us)" + maintainer = "Robert Griffin (rgriffin@ihmc.org)" configureDependencyResolution() configurePublications() diff --git a/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java index 47580e57..47bf4907 100644 --- a/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java +++ b/src/filters/java/us/ihmc/yoVariables/filters/AlphaFilteredYoMatrix.java @@ -3,6 +3,7 @@ import org.ejml.data.DMatrix; import org.ejml.data.DMatrixRMaj; import org.ejml.dense.row.CommonOps_DDRM; +import us.ihmc.yoVariables.math.YoMatrix; import us.ihmc.yoVariables.registry.YoRegistry; import us.ihmc.yoVariables.variable.YoDouble; diff --git a/src/filters/java/us/ihmc/yoVariables/filters/YoMatrix.java b/src/filters/java/us/ihmc/yoVariables/filters/YoMatrix.java deleted file mode 100644 index e640d87b..00000000 --- a/src/filters/java/us/ihmc/yoVariables/filters/YoMatrix.java +++ /dev/null @@ -1,352 +0,0 @@ -package us.ihmc.yoVariables.filters; - -import org.ejml.data.*; -import org.ejml.ops.MatrixIO; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.YoInteger; - -import java.io.ByteArrayOutputStream; -import java.io.PrintStream; - -/** - * YoMatrix. Object for holding a matrix of YoVariables so that Matrices can be rewound. Has a - * maximum number of rows and columns and an actual number of rows and columns. If you set with a - * smaller matrix, then the actual size will be the size of the passed in matrix. extra entries will - * be set to NaN. If you get the contents the matrix you pack must be the correct size. - * - * @author JerryPratt - */ -public class YoMatrix implements DMatrix, ReshapeMatrix -{ - // TODO: eventually consolidate YoMatrix implementations - - private static final long serialVersionUID = 2156411740647948028L; - - private final int maxNumberOfRows, maxNumberOfColumns; - - private final YoInteger numberOfRows, numberOfColumns; - private final YoDouble[][] variables; - - public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) - { - this(name, null, maxNumberOfRows, maxNumberOfColumns, null, null, registry); - } - - public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, YoRegistry registry) - { - this(name, null, maxNumberOfRows, maxNumberOfColumns, rowNames, null, registry); - } - - public YoMatrix(String name, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) - { - this(name, null, maxNumberOfRows, maxNumberOfColumns, rowNames, columnNames, registry); - } - - public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) - { - this(name, description, maxNumberOfRows, maxNumberOfColumns, null, null, registry); - } - - public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, YoRegistry registry) - { - this(name, description, maxNumberOfRows, maxNumberOfColumns, rowNames, null, registry); - } - - public YoMatrix(String name, String description, int maxNumberOfRows, int maxNumberOfColumns, String[] rowNames, String[] columnNames, YoRegistry registry) - { - this.maxNumberOfRows = maxNumberOfRows; - this.maxNumberOfColumns = maxNumberOfColumns; - - this.numberOfRows = new YoInteger(name + "NumRows", registry); - this.numberOfColumns = new YoInteger(name + "NumCols", registry); - - this.numberOfRows.set(maxNumberOfRows); - this.numberOfColumns.set(maxNumberOfColumns); - - variables = new YoDouble[maxNumberOfRows][maxNumberOfColumns]; - - for (int row = 0; row < maxNumberOfRows; row++) - { - for (int column = 0; column < maxNumberOfColumns; column++) - { - switch (checkNames(rowNames, columnNames)) - { - case NONE: - { - variables[row][column] = new YoDouble(getFieldName(name, row, column), description, registry); // names are simply the row and column indices - variables[row][column].setToNaN(); - break; - } - case ROWS: - { - if (maxNumberOfColumns > 1) - throw new IllegalArgumentException( - "The YoMatrix must be a column vector if only row names are provided, else unique names cannot be generated."); - - variables[row][column] = new YoDouble(getFieldName(name, rowNames[row], ""), description, registry); // names are the row names, no column identifier - variables[row][column].setToNaN(); - break; - } - case ROWS_AND_COLUMNS: - { - variables[row][column] = new YoDouble(getFieldName(name, rowNames[row], columnNames[column]), description, registry); // names are the row and column names - variables[row][column].setToNaN(); - break; - } - } - } - } - } - - public static String getFieldName(String prefix, int row, int column) - { - return getFieldName(prefix, "_" + row, "_" + column); - } - - public static String getFieldName(String prefix, String rowName, String columName) - { - return prefix + rowName + columName; - } - - private enum NamesProvided - { - NONE, ROWS, ROWS_AND_COLUMNS - } - - private NamesProvided checkNames(String[] rowNames, String[] columnNames) - { - if (rowNames == null && columnNames == null) - return NamesProvided.NONE; - else if (rowNames != null && columnNames == null) - return NamesProvided.ROWS; - else - return NamesProvided.ROWS_AND_COLUMNS; - } - - @Override - public double get(int row, int col) - { - if (col < 0 || col >= getNumCols() || row < 0 || row >= getNumRows()) - throw new IllegalArgumentException("Specified element is out of bounds: (" + row + " , " + col + ")"); - return unsafe_get(row, col); - } - - @Override - public double unsafe_get(int row, int col) - { - return variables[row][col].getValue(); - } - - @Override - public void set(int row, int col, double val) - { - if (col < 0 || col >= getNumCols() || row < 0 || row >= getNumRows()) - throw new IllegalArgumentException("Specified element is out of bounds: (" + row + " , " + col + ")"); - unsafe_set(row, col, val); - } - - @Override - public void unsafe_set(int row, int col, double val) - { - unsafe_set(row, col, val, true); - } - - private void unsafe_set(int row, int col, double val, boolean notifyListeners) - { - variables[row][col].set(val, notifyListeners); - } - - @Override - public int getNumElements() - { - return numberOfRows.getValue() * numberOfColumns.getValue(); - } - - @Override - public int getNumRows() - { - return numberOfRows.getValue(); - } - - @Override - public int getNumCols() - { - return numberOfColumns.getValue(); - } - - @Override - public void zero() - { - for (int row = 0; row < getNumRows(); row++) - { - for (int col = 0; col < getNumCols(); col++) - { - variables[row][col].set(0.0); - } - } - } - - @Override - public T copy() - { - throw new UnsupportedOperationException(); - } - - @Override - public T createLike() - { - throw new UnsupportedOperationException(); - } - - @Override - public T create(int numRows, int numCols) - { - throw new UnsupportedOperationException(); - } - - @Override - public void set(Matrix original) - { - if (original instanceof DMatrix otherMatrix) - { - reshape(otherMatrix.getNumRows(), otherMatrix.getNumCols()); - for (int row = 0; row < getNumRows(); row++) - { - for (int col = 0; col < getNumCols(); col++) - { - unsafe_set(row, col, otherMatrix.unsafe_get(row, col), false); - } - } - } - } - - @Override - public void print() - { - MatrixIO.printFancy(System.out, this, MatrixIO.DEFAULT_LENGTH); - } - - @Override - public void print(String format) - { - MatrixIO.print(System.out, this, format); - } - - @Override - public MatrixType getType() - { - return MatrixType.UNSPECIFIED; - } - - @Override - public void reshape(int numRows, int numCols) - { - if (numRows > maxNumberOfRows) - throw new IllegalArgumentException("Too many rows. Expected less or equal to " + maxNumberOfRows + ", was " + numRows); - else if (numCols > maxNumberOfColumns) - throw new IllegalArgumentException("Too many columns. Expected less or equal to " + maxNumberOfColumns + ", was " + numCols); - else if (numRows < 0 || numCols < 0) - throw new IllegalArgumentException("Cannot reshape with a negative number of rows or columns."); - - numberOfRows.set(numRows); - numberOfColumns.set(numCols); - - for (int row = 0; row < numRows; row++) - { - for (int col = numCols; col < maxNumberOfColumns; col++) - { - unsafe_set(row, col, Double.NaN, false); - } - } - - for (int row = numRows; row < maxNumberOfRows; row++) - { - for (int col = 0; col < maxNumberOfColumns; col++) - { - unsafe_set(row, col, Double.NaN, false); - } - } - } - - public void set(DMatrix matrix) - { - int numRows = matrix.getNumRows(); - int numCols = matrix.getNumCols(); - - if (((numRows > maxNumberOfRows) || (numCols > maxNumberOfColumns)) && (numRows > 0) && (numCols > 0)) - throw new RuntimeException("Not enough rows or columns. matrix to set is " + matrix.getNumRows() + " by " + matrix.getNumCols()); - - this.numberOfRows.set(numRows); - this.numberOfColumns.set(numCols); - - for (int row = 0; row < maxNumberOfRows; row++) - { - for (int column = 0; column < maxNumberOfColumns; column++) - { - double value; - if ((row < numRows) && (column < numCols)) - { - value = matrix.unsafe_get(row, column); - } - else - { - value = Double.NaN; - } - unsafe_set(row, column, value, false); - - } - } - } - - public void getAndReshape(DMatrixRMaj matrixToPack) - { - matrixToPack.reshape(getNumRows(), getNumCols()); - get(matrixToPack); - } - - public void get(DMatrix matrixToPack) - { - int numRows = matrixToPack.getNumRows(); - int numCols = matrixToPack.getNumCols(); - - if (((numRows > maxNumberOfRows) || (numCols > maxNumberOfColumns)) && (numRows > 0) && (numCols > 0)) - throw new RuntimeException("Not enough rows or columns. matrixToPack is " + matrixToPack.getNumRows() + " by " + matrixToPack.getNumCols()); - if ((numRows != this.numberOfRows.getIntegerValue()) || (numCols != this.numberOfColumns.getIntegerValue())) - throw new RuntimeException("Numer of rows and columns must be the same. Call getAndReshape() if you want to reshape the matrixToPack"); - - for (int row = 0; row < numRows; row++) - { - for (int column = 0; column < numCols; column++) - { - matrixToPack.unsafe_set(row, column, variables[row][column].getDoubleValue()); - } - } - } - - public void setToNaN(int numberOfRows, int numberOfColumns) - { - reshape(numberOfRows, numberOfColumns); - for (int row = 0; row < numberOfRows; row++) - { - for (int col = 0; col < numberOfColumns; col++) - { - unsafe_set(row, col, Double.NaN, false); - } - } - } - - public YoDouble getYoDouble(int row, int col) - { - return variables[row][col]; - } - - @Override - public String toString() - { - ByteArrayOutputStream stream = new ByteArrayOutputStream(); - MatrixIO.print(new PrintStream(stream), this); - - return stream.toString(); - } -} diff --git a/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java b/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java index 47a3599b..e7327a45 100644 --- a/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java +++ b/src/main/java/us/ihmc/yoVariables/math/YoMatrix.java @@ -396,6 +396,40 @@ public void get(DMatrix matrixToPack) } } + /** + * Sets the content of {@code this} matrix to be equivalent to the {@code matrix} + * + * @param matrix the matrix to set {@code this} to. Modified. + */ + public void set(DMatrix matrix) + { + int numRows = matrix.getNumRows(); + int numCols = matrix.getNumCols(); + + if (((numRows > maxNumberOfRows) || (numCols > maxNumberOfColumns)) && (numRows > 0) && (numCols > 0)) + throw new RuntimeException("Not enough rows or columns. matrix to set is " + matrix.getNumRows() + " by " + matrix.getNumCols()); + + this.numberOfRows.set(numRows); + this.numberOfColumns.set(numCols); + + for (int row = 0; row < maxNumberOfRows; row++) + { + for (int column = 0; column < maxNumberOfColumns; column++) + { + double value; + if ((row < numRows) && (column < numCols)) + { + value = matrix.unsafe_get(row, column); + } + else + { + value = Double.NaN; + } + unsafe_set(row, column, value, false); + } + } + } + /** * Set the number of rows and columns in the matrix. *

diff --git a/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java b/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java deleted file mode 100644 index 9847d094..00000000 --- a/src/test/java/us/ihmc/yoVariables/euclid/filters/YoMatrixTest.java +++ /dev/null @@ -1,114 +0,0 @@ -package us.ihmc.yoVariables.euclid.filters; - -import java.util.Random; - -import org.ejml.EjmlUnitTests; -import org.ejml.data.DMatrixRMaj; -import org.ejml.dense.row.RandomMatrices_DDRM; -import org.junit.jupiter.api.Test; - -import us.ihmc.yoVariables.filters.YoMatrix; -import us.ihmc.yoVariables.registry.YoRegistry; -import us.ihmc.yoVariables.variable.YoDouble; - -import static org.junit.jupiter.api.Assertions.*; - -public class YoMatrixTest -{ - - - @Test - public void testYoMatrixSetTooBig() - { - int maxNumberOfRows = 4; - int maxNumberOfColumns = 8; - String name = "testMatrix"; - YoRegistry registry = new YoRegistry("testRegistry"); - YoMatrix yoMatrix = new YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry); - - DMatrixRMaj tooBigMatrix = new DMatrixRMaj(maxNumberOfRows + 1, maxNumberOfColumns); - - try - { - yoMatrix.set(tooBigMatrix); - fail("Too Big"); - } - catch (RuntimeException e) - { - } - - tooBigMatrix = new DMatrixRMaj(maxNumberOfRows, maxNumberOfColumns + 1); - - try - { - yoMatrix.set(tooBigMatrix); - fail("Too Big"); - } - catch (RuntimeException e) - { - } - - // Test a 0 X Big Matrix - DMatrixRMaj okMatrix = new DMatrixRMaj(0, maxNumberOfColumns + 10); - yoMatrix.set(okMatrix); - assertMatrixYoVariablesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, registry); - - DMatrixRMaj checkMatrix = new DMatrixRMaj(1, 1); - yoMatrix.getAndReshape(checkMatrix); - - assertEquals(0, checkMatrix.getNumRows()); - assertEquals(maxNumberOfColumns + 10, checkMatrix.getNumCols()); - - // Test a Big X 0 Matrix - - okMatrix = new DMatrixRMaj(maxNumberOfRows + 10, 0); - yoMatrix.set(okMatrix); - assertMatrixYoVariablesAreNaN(name, maxNumberOfRows, maxNumberOfColumns, registry); - - checkMatrix = new DMatrixRMaj(1, 1); - yoMatrix.getAndReshape(checkMatrix); - - assertEquals(maxNumberOfRows + 10, checkMatrix.getNumRows()); - assertEquals(0, checkMatrix.getNumCols()); - - } - - - private void checkMatrixYoVariablesEqualsCheckMatrixAndOutsideValuesAreNaN(String name, int maxNumberOfRows, int maxNumberOfColumns, DMatrixRMaj checkMatrix, YoRegistry registry) - { - int smallerRows = checkMatrix.getNumRows(); - int smallerColumns = checkMatrix.getNumCols(); - - // Make sure the values are correct, including values outside the range should be NaN: - for (int row = 0; row < maxNumberOfRows; row++) - { - for (int column = 0; column < maxNumberOfColumns; column++) - { - YoDouble variable = (YoDouble) registry.findVariable(YoMatrix.getFieldName(name, row, column)); - - if ((row < smallerRows) && (column < smallerColumns)) - { - assertEquals(checkMatrix.get(row, column), variable.getDoubleValue(), 1e-10); - } - else - { - assertTrue(Double.isNaN(variable.getDoubleValue()), "Values outside aren't NaN, instead are " + variable.getDoubleValue()); - } - - } - } - } - - private void assertMatrixYoVariablesAreNaN(String name, int maxNumberOfRows, int maxNumberOfColumns, YoRegistry registry) - { - for (int row = 0; row < maxNumberOfRows; row++) - { - for (int column = 0; column < maxNumberOfColumns; column++) - { - YoDouble variable = (YoDouble) registry.findVariable(YoMatrix.getFieldName(name, row, column)); - assertTrue(Double.isNaN(variable.getDoubleValue())); - } - } - } - -} diff --git a/src/test/java/us/ihmc/yoVariables/math/YoMatrixTest.java b/src/test/java/us/ihmc/yoVariables/math/YoMatrixTest.java index c4a6df2f..2cc87907 100644 --- a/src/test/java/us/ihmc/yoVariables/math/YoMatrixTest.java +++ b/src/test/java/us/ihmc/yoVariables/math/YoMatrixTest.java @@ -54,7 +54,7 @@ public void testSimpleYoMatrixExample() int maxNumberOfRows = 4; int maxNumberOfColumns = 8; YoRegistry registry = new YoRegistry("testRegistry"); - us.ihmc.yoVariables.filters.YoMatrix yoMatrix = new us.ihmc.yoVariables.filters.YoMatrix("testMatrix", maxNumberOfRows, maxNumberOfColumns, registry); + YoMatrix yoMatrix = new YoMatrix("testMatrix", maxNumberOfRows, maxNumberOfColumns, registry); assertEquals(maxNumberOfRows, yoMatrix.getNumRows()); assertEquals(maxNumberOfColumns, yoMatrix.getNumCols()); @@ -76,7 +76,7 @@ public void testSimpleYoMatrixExample() EjmlUnitTests.assertEquals(randomMatrix, checkMatrix, 1e-10); - assertEquals(registry.findVariable(us.ihmc.yoVariables.filters.YoMatrix.getFieldName("testMatrix", 0, 0)).getValueAsDouble(), checkMatrix.get(0, 0), 1e-10); + assertEquals(registry.findVariable(YoMatrix.getFieldName("testMatrix", 0, 0)).getValueAsDouble(), checkMatrix.get(0, 0), 1e-10); } @@ -320,7 +320,7 @@ public void testYoMatrixSetTooBig() int maxNumberOfColumns = 8; String name = "testMatrix"; YoRegistry registry = new YoRegistry("testRegistry"); - us.ihmc.yoVariables.filters.YoMatrix yoMatrix = new us.ihmc.yoVariables.filters.YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry); + YoMatrix yoMatrix = new YoMatrix(name, maxNumberOfRows, maxNumberOfColumns, registry); DMatrixRMaj tooBigMatrix = new DMatrixRMaj(maxNumberOfRows + 1, maxNumberOfColumns); @@ -376,7 +376,7 @@ private static void assertMatrixYoVariablesAreNaN(String name, int maxNumberOfRo { for (int column = 0; column < maxNumberOfColumns; column++) { - YoDouble variable = (YoDouble) registry.findVariable(us.ihmc.yoVariables.filters.YoMatrix.getFieldName(name, row, column)); + YoDouble variable = (YoDouble) registry.findVariable(YoMatrix.getFieldName(name, row, column)); assertTrue(Double.isNaN(variable.getDoubleValue())); } }