Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add core State-space classes (#2614)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Claudius Tewari <cttewari@gmail.com>
Co-authored-by: Declan Freeman-Gleason <declanfreemangleason@gmail.com>
This commit is contained in:
Matt
2020-08-14 23:40:33 -07:00
committed by GitHub
parent e5b84e2f87
commit 3b283ab9aa
84 changed files with 11747 additions and 174 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java
View File

@@ -18,8 +18,8 @@ import org.ejml.simple.SimpleMatrix;
* @param <C> The number of columns of the desired matrix.
*/
public class MatBuilder<R extends Num, C extends Num> {
private final Nat<R> m_rows;
private final Nat<C> m_cols;
final Nat<R> m_rows;
final Nat<C> m_cols;
/**
* Fills the matrix with the given data, encoded in row major form.

440
wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
View File

@@ -9,10 +9,17 @@ package edu.wpi.first.wpiutil.math;
import java.util.Objects;
import org.ejml.MatrixDimensionException;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.MatrixFeatures_DDRM;
import org.ejml.dense.row.NormOps_DDRM;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpiutil.math.numbers.N1;
/**
* A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
*
@@ -21,10 +28,60 @@ import org.ejml.simple.SimpleMatrix;
* @param <R> The number of rows in this matrix.
* @param <C> The number of columns in this matrix.
*/
@SuppressWarnings("PMD.TooManyMethods")
@SuppressWarnings({"PMD.TooManyMethods", "PMD.ExcessivePublicCount"})
public class Matrix<R extends Num, C extends Num> {
protected final SimpleMatrix m_storage;
private final SimpleMatrix m_storage;
/**
* Constructs an empty zero matrix of the given dimensions.
*
* @param rows The number of rows of the matrix.
* @param columns The number of columns of the matrix.
*/
public Matrix(Nat<R> rows, Nat<C> columns) {
this.m_storage = new SimpleMatrix(
Objects.requireNonNull(rows).getNum(),
Objects.requireNonNull(columns).getNum()
);
}
/**
* Constructs a new {@link Matrix} with the given storage.
* Caller should make sure that the provided generic bounds match
* the shape of the provided {@link Matrix}.
*
* <p>NOTE:It is not recommend to use this constructor unless the
* {@link SimpleMatrix} API is absolutely necessary due to the desired
* function not being accessible through the {@link Matrix} wrapper.
*
* @param storage The {@link SimpleMatrix} to back this value.
*/
public Matrix(SimpleMatrix storage) {
this.m_storage = Objects.requireNonNull(storage);
}
/**
* Constructs a new matrix with the storage of the supplied matrix.
*
* @param other The {@link Matrix} to copy the storage of.
*/
public Matrix(Matrix<R, C> other) {
this.m_storage = Objects.requireNonNull(other).getStorage().copy();
}
/**
* Gets the underlying {@link SimpleMatrix} that this {@link Matrix} wraps.
*
* <p>NOTE:The use of this method is heavily discouraged as this removes any
* guarantee of type safety. This should only be called if the {@link SimpleMatrix}
* API is absolutely necessary due to the desired function not being accessible through
* the {@link Matrix} wrapper.
*
* @return The underlying {@link SimpleMatrix} storage.
*/
public SimpleMatrix getStorage() {
return m_storage;
}
/**
* Gets the number of columns in this matrix.
@@ -58,8 +115,8 @@ public class Matrix<R extends Num, C extends Num> {
/**
* Sets the value at the given indices.
*
* @param row The row of the element.
* @param col The column of the element.
* @param row The row of the element.
* @param col The column of the element.
* @param value The value to insert at the given location.
*/
public final void set(int row, int col, double value) {
@@ -67,10 +124,44 @@ public class Matrix<R extends Num, C extends Num> {
}
/**
* If a vector then a square matrix is returned
* if a matrix then a vector of diagonal elements is returned.
* Sets a row to a given row vector.
*
* @return Diagonal elements inside a vector or a square matrix with the same diagonal elements.
* @param row The row to set.
* @param val The row vector to set the given row to.
*/
public final void setRow(int row, Matrix<N1, C> val) {
this.m_storage.setRow(row, 0,
Objects.requireNonNull(val).m_storage.getDDRM().getData());
}
/**
* Sets a column to a given column vector.
*
* @param column The column to set.
* @param val The column vector to set the given row to.
*/
public final void setColumn(int column, Matrix<R, N1> val) {
this.m_storage.setColumn(column, 0,
Objects.requireNonNull(val).m_storage.getDDRM().getData());
}
/**
* Sets all the elements in "this" matrix equal to the specified value.
*
* @param value The value each element is set to.
*/
public void fill(double value) {
this.m_storage.fill(value);
}
/**
* Returns the diagonal elements inside a vector or square matrix.
*
* <p>If "this" {@link Matrix} is a vector then a square matrix is returned. If a "this"
* {@link Matrix} is a matrix then a vector of diagonal elements is returned.
*
* @return The diagonal elements inside a vector or a square matrix.
*/
public final Matrix<R, C> diag() {
return new Matrix<>(this.m_storage.diag());
@@ -81,10 +172,20 @@ public class Matrix<R extends Num, C extends Num> {
*
* @return The largest element of this matrix.
*/
public final double maxInternal() {
public final double max() {
return CommonOps_DDRM.elementMax(this.m_storage.getDDRM());
}
/**
* Returns the absolute value of the element in this matrix with the largest absolute value.
*
* @return The absolute value of the element with the largest absolute value.
*/
public final double maxAbs() {
return CommonOps_DDRM.elementMaxAbs(this.m_storage.getDDRM());
}
/**
* Returns the smallest element of this matrix.
*
@@ -112,10 +213,10 @@ public class Matrix<R extends Num, C extends Num> {
*
* @param other The other matrix to multiply by.
* @param <C2> The number of columns in the second matrix.
* @return The result of the matrix multiplication between this and the given matrix.
* @return The result of the matrix multiplication between "this" and the given matrix.
*/
public final <C2 extends Num> Matrix<R, C2> times(Matrix<C, C2> other) {
return new Matrix<>(this.m_storage.mult(other.m_storage));
return new Matrix<>(this.m_storage.mult(Objects.requireNonNull(other).m_storage));
}
/**
@@ -129,13 +230,14 @@ public class Matrix<R extends Num, C extends Num> {
}
/**
* <p>
* Returns a matrix which is the result of an element by element multiplication of 'this' and 'b'.
* c<sub>i,j</sub> = a<sub>i,j</sub>*b<sub>i,j</sub>
* </p>
* Returns a matrix which is the result of an element by element multiplication of
* "this" and other.
*
* @param other A matrix.
* @return The element by element multiplication of 'this' and 'b'.
* <p>c<sub>i,j</sub> = a<sub>i,j</sub>*other<sub>i,j</sub>
*
*
* @param other The other {@link Matrix} to preform element multiplication on.
* @return The element by element multiplication of "this" and other.
*/
public final Matrix<R, C> elementTimes(Matrix<R, C> other) {
return new Matrix<>(this.m_storage.elementMult(Objects.requireNonNull(other).m_storage));
@@ -180,7 +282,7 @@ public class Matrix<R extends Num, C extends Num> {
* @return The resultant matrix.
*/
public final Matrix<R, C> plus(Matrix<R, C> value) {
return new Matrix<>(this.m_storage.plus(value.m_storage));
return new Matrix<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
}
/**
@@ -206,7 +308,7 @@ public class Matrix<R extends Num, C extends Num> {
/**
* Calculates the transpose, M^T of this matrix.
*
* @return The tranpose matrix.
* @return The transpose matrix.
*/
public final Matrix<C, R> transpose() {
return new Matrix<>(this.m_storage.transpose());
@@ -224,15 +326,47 @@ public class Matrix<R extends Num, C extends Num> {
/**
* Returns the inverse matrix of this matrix.
* Returns the inverse matrix of "this" matrix.
*
* @return The inverse of this matrix.
* @throws org.ejml.data.SingularMatrixException If this matrix is non-invertable.
* @return The inverse of "this" matrix.
* @throws org.ejml.data.SingularMatrixException If "this" matrix is non-invertable.
*/
public final Matrix<R, C> inv() {
return new Matrix<>(this.m_storage.invert());
}
/**
* Returns the solution x to the equation Ax = b, where A is "this" matrix.
*
* <p>The matrix equation could also be written as x = A<sup>-1</sup>b. Where the
* pseudo inverse is used if A is not square.
*
* @param b The right-hand side of the equation to solve.
* @return The solution to the linear system.
*/
@SuppressWarnings("ParameterName")
public final <C2 extends Num> Matrix<C, C2> solve(Matrix<R, C2> b) {
return new Matrix<>(this.m_storage.solve(Objects.requireNonNull(b).m_storage));
}
/**
* Computes the matrix exponential using Eigen's solver.
* This method only works for square matrices, and will
* otherwise throw an {@link MatrixDimensionException}.
*
* @return The exponential of A.
*/
public final Matrix<R, C> exp() {
if (this.getNumRows() != this.getNumCols()) {
throw new MatrixDimensionException("Non-square matrices cannot be exponentiated! "
+ "This matrix is " + this.getNumRows() + " x " + this.getNumCols());
}
Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
WPIMathJNI.exp(this.m_storage.getDDRM().getData(), this.getNumRows(),
toReturn.m_storage.getDDRM().getData());
return toReturn;
}
/**
* Returns the determinant of this matrix.
*
@@ -243,9 +377,9 @@ public class Matrix<R extends Num, C extends Num> {
}
/**
* Computes the Frobenius normal of the matrix.<br>
* <br>
* normF = Sqrt{ &sum;<sub>i=1:m</sub> &sum;<sub>j=1:n</sub> { a<sub>ij</sub><sup>2</sup>} }
* Computes the Frobenius normal of the matrix.
*
* <p>normF = Sqrt{ &sum;<sub>i=1:m</sub> &sum;<sub>j=1:n</sub> { a<sub>ij</sub><sup>2</sup>} }
*
* @return The matrix's Frobenius normal.
*/
@@ -254,9 +388,9 @@ public class Matrix<R extends Num, C extends Num> {
}
/**
* Computes the induced p = 1 matrix norm.<br>
* <br>
* ||A||<sub>1</sub>= max(j=1 to n; sum(i=1 to m; |a<sub>ij</sub>|))
* Computes the induced p = 1 matrix norm.
*
* <p>||A||<sub>1</sub>= max(j=1 to n; sum(i=1 to m; |a<sub>ij</sub>|))
*
* @return The norm.
*/
@@ -283,45 +417,261 @@ public class Matrix<R extends Num, C extends Num> {
}
/**
* Returns a matrix which is the result of an element by element power of 'this' and 'b':
* c<sub>i,j</sub> = a<sub>i,j</sub> ^ b.
* Returns a matrix which is the result of an element by element power of "this" and b.
*
* @param b Scalar
* @return The element by element power of 'this' and 'b'.
* <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
*
* @param b Scalar.
* @return The element by element power of "this" and b.
*/
@SuppressWarnings("ParameterName")
public final Matrix<R, C> epow(double b) {
public final Matrix<R, C> elementPower(double b) {
return new Matrix<>(this.m_storage.elementPower(b));
}
/**
* Returns a matrix which is the result of an element by element power of 'this' and 'b':
* c<sub>i,j</sub> = a<sub>i,j</sub> ^ b.
* Returns a matrix which is the result of an element by element power of "this" and b.
*
* <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
*
* @param b Scalar.
* @return The element by element power of 'this' and 'b'.
* @return The element by element power of "this" and b.
*/
@SuppressWarnings("ParameterName")
public final Matrix<R, C> epow(int b) {
public final Matrix<R, C> elementPower(int b) {
return new Matrix<>(this.m_storage.elementPower((double) b));
}
/**
* Returns the EJML {@link SimpleMatrix} backing this wrapper.
* Extracts a given row into a row vector with new underlying storage.
*
* @return The untyped EJML {@link SimpleMatrix}.
* @param row The row to extract a vector from.
* @return A row vector from the given row.
*/
public final SimpleMatrix getStorage() {
return this.m_storage;
public final Matrix<N1, C> extractRowVector(int row) {
return new Matrix<>(this.m_storage.extractVector(true, row));
}
/**
* Constructs a new matrix with the given storage.
* Caller should make sure that the provided generic bounds match the shape of the provided matrix
* Extracts a given column into a column vector with new underlying storage.
*
* @param storage The {@link SimpleMatrix} to back this value
* @param column The column to extract a vector from.
* @return A column vector from the given column.
*/
public Matrix(SimpleMatrix storage) {
this.m_storage = Objects.requireNonNull(storage);
public final Matrix<R, N1> extractColumnVector(int column) {
return new Matrix<>(this.m_storage.extractVector(false, column));
}
/**
* Extracts a matrix of a given size and start position with new underlying
* storage.
*
* @param height The number of rows of the extracted matrix.
* @param width The number of columns of the extracted matrix.
* @param startingRow The starting row of the extracted matrix.
* @param startingCol The starting column of the extracted matrix.
* @return The extracted matrix.
*/
public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
Nat<R2> height, Nat<C2> width, int startingRow, int startingCol) {
return new Matrix<>(this.m_storage.extractMatrix(
startingRow,
Objects.requireNonNull(height).getNum() + startingRow,
startingCol,
Objects.requireNonNull(width).getNum() + startingCol));
}
/**
* Assign a matrix of a given size and start position.
*
* @param startingRow The row to start at.
* @param startingCol The column to start at.
* @param other The matrix to assign the block to.
*/
public <R2 extends Num, C2 extends Num> void assignBlock(int startingRow, int startingCol,
Matrix<R2, C2> other) {
this.m_storage.insertIntoThis(
startingRow,
startingCol,
Objects.requireNonNull(other).m_storage);
}
/**
* Extracts a submatrix from the supplied matrix and inserts it in a submatrix in "this". The
* shape of "this" is used to determine the size of the matrix extracted.
*
* @param startingRow The starting row in the supplied matrix to extract the submatrix.
* @param startingCol The starting column in the supplied matrix to extract the submatrix.
* @param other The matrix to extract the submatrix from.
*/
public <R2 extends Num, C2 extends Num> void extractFrom(int startingRow, int startingCol,
Matrix<R2, C2> other) {
CommonOps_DDRM.extract(other.m_storage.getDDRM(), startingRow, startingCol,
this.m_storage.getDDRM());
}
/**
* Decompose "this" matrix using Cholesky Decomposition. If the "this" matrix is zeros, it
* will return the zero matrix.
*
* @param lowerTriangular Whether or not we want to decompose to the lower triangular
* Cholesky matrix.
* @return The decomposed matrix.
* @throws RuntimeException if the matrix could not be decomposed(ie. is not positive
* semidefinite).
*/
@SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes")
public Matrix<R, C> lltDecompose(boolean lowerTriangular) {
SimpleMatrix temp = m_storage.copy();
CholeskyDecomposition_F64<DMatrixRMaj> chol =
DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
if (!chol.decompose(temp.getMatrix())) {
// check that the input is not all zeros -- if they are, we special case and return all
// zeros.
var matData = temp.getDDRM().data;
var isZeros = true;
for (double matDatum : matData) {
isZeros &= Math.abs(matDatum) < 1e-6;
}
if (isZeros) {
return new Matrix<>(new SimpleMatrix(temp.numRows(), temp.numCols()));
}
throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n"
+ m_storage.toString());
}
return new Matrix<>(SimpleMatrix.wrap(chol.getT(null)));
}
/**
* Returns the row major data of this matrix as a double array.
*
* @return The row major data of this matrix as a double array.
*/
public double[] getData() {
return m_storage.getDDRM().getData();
}
/**
* Creates the identity matrix of the given dimension.
*
* @param dim The dimension of the desired matrix as a {@link Nat}.
* @param <D> The dimension of the desired matrix as a generic.
* @return The DxD identity matrix.
*/
public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
}
/**
* Creates the identity matrix of the given dimension.
*
* @param dim The dimension of the desired matrix as a {@link Num}.
* @param <D> The dimension of the desired matrix as a generic.
* @return The DxD identity matrix.
*/
public static <D extends Num> Matrix<D, D> eye(D dim) {
return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
}
/**
* Entrypoint to the {@link MatBuilder} class for creation
* of custom matrices with the given dimensions and contents.
*
* @param rows The number of rows of the desired matrix.
* @param cols The number of columns of the desired matrix.
* @param <R> The number of rows of the desired matrix as a generic.
* @param <C> The number of columns of the desired matrix as a generic.
* @return A builder to construct the matrix.
*/
public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
return new MatBuilder<>(Objects.requireNonNull(rows), Objects.requireNonNull(cols));
}
/**
* Reassigns dimensions of a {@link Matrix} to allow for operations with
* other matrices that have wildcard dimensions.
*
* @param mat The {@link Matrix} to remove the dimensions from.
* @return The matrix with reassigned dimensions.
*/
public static <R1 extends Num, C1 extends Num> Matrix<R1, C1> changeBoundsUnchecked(
Matrix<?, ?> mat) {
return new Matrix<>(mat.m_storage);
}
/**
* Checks if another {@link Matrix} is identical to "this" one within a specified tolerance.
*
* <p>This will check if each element is in tolerance of the corresponding element
* from the other {@link Matrix} or if the elements have the same symbolic meaning. For two
* elements to have the same symbolic meaning they both must be either Double.NaN,
* Double.POSITIVE_INFINITY, or Double.NEGATIVE_INFINITY.
*
* <p>NOTE:It is recommend to use {@link Matrix#isEqual(Matrix, double)} over this
* method when checking if two matrices are equal as {@link Matrix#isEqual(Matrix, double)}
* will return false if an element is uncountable. This method should only be used when
* uncountable elements need to compared.
*
* @param other The {@link Matrix} to check against this one.
* @param tolerance The tolerance to check equality with.
* @return true if this matrix is identical to the one supplied.
*/
public boolean isIdentical(Matrix<?, ?> other, double tolerance) {
return MatrixFeatures_DDRM.isIdentical(this.m_storage.getDDRM(),
other.m_storage.getDDRM(), tolerance);
}
/**
* Checks if another {@link Matrix} is equal to "this" within a specified tolerance.
*
* <p>This will check if each element is in tolerance of the corresponding element
* from the other {@link Matrix}.
*
* <p>tol &ge; |a<sub>ij</sub> - b<sub>ij</sub>|
*
* @param other The {@link Matrix} to check against this one.
* @param tolerance The tolerance to check equality with.
* @return true if this matrix is equal to the one supplied.
*/
public boolean isEqual(Matrix<?, ?> other, double tolerance) {
return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(),
other.m_storage.getDDRM(), tolerance);
}
@Override
public String toString() {
return m_storage.toString();
}
/**
* Checks if an object is equal to this {@link Matrix}.
*
* <p>a<sub>ij</sub> == b<sub>ij</sub>
*
* @param other The Object to check against this {@link Matrix}.
* @return true if the object supplied is a {@link Matrix} and is equal to this matrix.
*/
@Override
public boolean equals(Object other) {
if (this == other) {
return true;
}
if (!(other instanceof Matrix)) {
return false;
}
Matrix<?, ?> matrix = (Matrix<?, ?>) other;
if (MatrixFeatures_DDRM.hasUncountable(matrix.m_storage.getDDRM())) {
return false;
}
return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(), matrix.m_storage.getDDRM());
}
@Override
public int hashCode() {
return Objects.hash(m_storage);
}
}

1
wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java
View File

@@ -13,6 +13,7 @@ import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpiutil.math.numbers.N1;
@Deprecated
public final class MatrixUtils {
private MatrixUtils() {
throw new AssertionError("utility class");

31
wpimath/src/main/java/edu/wpi/first/wpiutil/math/Pair.java Normal file
View File

@@ -0,0 +1,31 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpiutil.math;
public class Pair<A, B> {
private final A m_first;
private final B m_second;
public Pair(A first, B second) {
m_first = first;
m_second = second;
}
public A getFirst() {
return m_first;
}
public B getSecond() {
return m_second;
}
@SuppressWarnings("ParameterName")
public static <A, B> Pair<A, B> of(A a, B b) {
return new Pair<>(a, b);
}
}

114
wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java
View File

@@ -9,12 +9,17 @@ package edu.wpi.first.wpiutil.math;
import java.util.function.BiFunction;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.NormOps_DDRM;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
import org.ejml.simple.SimpleBase;
import org.ejml.simple.SimpleMatrix;
public class SimpleMatrixUtils {
private SimpleMatrixUtils() {}
@SuppressWarnings("PMD.TooManyMethods")
public final class SimpleMatrixUtils {
private SimpleMatrixUtils() {
}
/**
* Compute the matrix exponential, e^M of the given matrix.
@@ -98,8 +103,10 @@ public class SimpleMatrixUtils {
SimpleMatrix A4 = A2.mult(A2);
SimpleMatrix A6 = A4.mult(A2);
SimpleMatrix U = A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
SimpleMatrix V = A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
SimpleMatrix U =
A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
SimpleMatrix V =
A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
return new Pair<>(U, V);
}
@@ -114,8 +121,10 @@ public class SimpleMatrixUtils {
SimpleMatrix A6 = A4.mult(A2);
SimpleMatrix A8 = A6.mult(A2);
SimpleMatrix U = A.mult(A8.scale(b[9]).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
SimpleMatrix V = A8.scale(b[8]).plus(A6.scale(b[6])).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
SimpleMatrix U =
A.mult(A8.scale(b[9]).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
SimpleMatrix V =
A8.scale(b[8]).plus(A6.scale(b[6])).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
return new Pair<>(U, V);
}
@@ -131,8 +140,10 @@ public class SimpleMatrixUtils {
SimpleMatrix A4 = A2.mult(A2);
SimpleMatrix A6 = A4.mult(A2);
SimpleMatrix U = A.mult(A6.scale(b[13]).plus(A4.scale(b[11])).plus(A2.scale(b[9])).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
SimpleMatrix V = A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8]))).plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])));
SimpleMatrix U =
A.mult(A6.scale(b[13]).plus(A4.scale(b[11])).plus(A2.scale(b[9])).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
SimpleMatrix V =
A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8]))).plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])));
return new Pair<>(U, V);
}
@@ -141,21 +152,76 @@ public class SimpleMatrixUtils {
return SimpleMatrix.identity(Math.min(rows, cols));
}
private static class Pair<A, B> {
private final A m_first;
private final B m_second;
Pair(A first, B second) {
m_first = first;
m_second = second;
}
public A getFirst() {
return m_first;
}
public B getSecond() {
return m_second;
}
/**
* The identy of a square matrix.
*
* @param rows the number of rows (and columns)
* @return the identiy matrix, rows x rows.
*/
public static SimpleMatrix eye(int rows) {
return SimpleMatrix.identity(rows);
}
/**
* Decompose the given matrix using Cholesky Decomposition and return a view of the upper
* triangular matrix (if you want lower triangular see the other overload of this method.) If the
* input matrix is zeros, this will return the zero matrix.
*
* @param src The matrix to decompose.
* @return The decomposed matrix.
* @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
* semidefinite).
*/
public static SimpleMatrix lltDecompose(SimpleMatrix src) {
return lltDecompose(src, false);
}
/**
* Decompose the given matrix using Cholesky Decomposition. If the input matrix is zeros, this
* will return the zero matrix.
*
* @param src The matrix to decompose.
* @param lowerTriangular if we want to decompose to the lower triangular Cholesky matrix.
* @return The decomposed matrix.
* @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
* semidefinite).
*/
@SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes")
public static SimpleMatrix lltDecompose(SimpleMatrix src, boolean lowerTriangular) {
SimpleMatrix temp = src.copy();
CholeskyDecomposition_F64<DMatrixRMaj> chol =
DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
if (!chol.decompose(temp.getMatrix())) {
// check that the input is not all zeros -- if they are, we special case and return all
// zeros.
var matData = temp.getDDRM().data;
var isZeros = true;
for (double matDatum : matData) {
isZeros &= Math.abs(matDatum) < 1e-6;
}
if (isZeros) {
return new SimpleMatrix(temp.numRows(), temp.numCols());
}
throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + src.toString());
}
return SimpleMatrix.wrap(chol.getT(null));
}
/**
* Computes the matrix exponential using Eigen's solver.
*
* @param A the matrix to exponentiate.
* @return the exponential of A.
*/
@SuppressWarnings("ParameterName")
public static SimpleMatrix exp(
SimpleMatrix A) {
SimpleMatrix toReturn = new SimpleMatrix(A.numRows(), A.numRows());
WPIMathJNI.exp(A.getDDRM().getData(), A.numRows(), toReturn.getDDRM().getData());
return toReturn;
}
}

157
wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java
View File

@@ -8,14 +8,171 @@
package edu.wpi.first.wpiutil.math;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N10;
import edu.wpi.first.wpiutil.math.numbers.N2;
import edu.wpi.first.wpiutil.math.numbers.N3;
import edu.wpi.first.wpiutil.math.numbers.N4;
import edu.wpi.first.wpiutil.math.numbers.N5;
import edu.wpi.first.wpiutil.math.numbers.N6;
import edu.wpi.first.wpiutil.math.numbers.N7;
import edu.wpi.first.wpiutil.math.numbers.N8;
import edu.wpi.first.wpiutil.math.numbers.N9;
/**
* A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
*
* @param <N> The dimension of the vector to be constructed.
*/
@SuppressWarnings("PMD.TooManyMethods")
public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
public VecBuilder(Nat<N> rows) {
super(rows, Nat.N1());
}
private Vector<N> fillVec(double... data) {
return new Vector<>(fill(data));
}
/**
* Returns a 1x1 vector containing the given elements.
*
* @param n1 the first element.
*/
public static Vector<N1> fill(double n1) {
return new VecBuilder<>(Nat.N1()).fillVec(n1);
}
/**
* Returns a 2x1 vector containing the given elements.
*
* @param n1 the first element.
* @param n2 the second element.
*/
public static Vector<N2> fill(double n1, double n2) {
return new VecBuilder<>(Nat.N2()).fillVec(n1, n2);
}
/**
* Returns a 3x1 vector containing the given elements.
*
* @param n1 the first element.
* @param n2 the second element.
* @param n3 the third element.
*/
public static Vector<N3> fill(double n1, double n2, double n3) {
return new VecBuilder<>(Nat.N3()).fillVec(n1, n2, n3);
}
/**
* Returns a 4x1 vector containing the given elements.
*
* @param n1 the first element.
* @param n2 the second element.
* @param n3 the third element.
* @param n4 the fourth element.
*/
public static Vector<N4> fill(double n1, double n2, double n3, double n4) {
return new VecBuilder<>(Nat.N4()).fillVec(n1, n2, n3, n4);
}
/**
* Returns a 5x1 vector containing the given elements.
*
* @param n1 the first element.
* @param n2 the second element.
* @param n3 the third element.
* @param n4 the fourth element.
* @param n5 the fifth element.
*/
public static Vector<N5> fill(double n1, double n2, double n3, double n4, double n5) {
return new VecBuilder<>(Nat.N5()).fillVec(n1, n2, n3, n4, n5);
}
/**
* Returns a 6x1 vector containing the given elements.
*
* @param n1 the first element.
* @param n2 the second element.
* @param n3 the third element.
* @param n4 the fourth element.
* @param n5 the fifth element.
* @param n6 the sixth element.
*/
public static Vector<N6> fill(double n1, double n2, double n3, double n4, double n5,
double n6) {
return new VecBuilder<>(Nat.N6()).fillVec(n1, n2, n3, n4, n5, n6);
}
/**
* Returns a 7x1 vector containing the given elements.
*
* @param n1 the first element.
* @param n2 the second element.
* @param n3 the third element.
* @param n4 the fourth element.
* @param n5 the fifth element.
* @param n6 the sixth element.
* @param n7 the seventh element.
*/
public static Vector<N7> fill(double n1, double n2, double n3, double n4, double n5,
double n6, double n7) {
return new VecBuilder<>(Nat.N7()).fillVec(n1, n2, n3, n4, n5, n6, n7);
}
/**
* Returns a 8x1 vector containing the given elements.
*
* @param n1 the first element.
* @param n2 the second element.
* @param n3 the third element.
* @param n4 the fourth element.
* @param n5 the fifth element.
* @param n6 the sixth element.
* @param n7 the seventh element.
* @param n8 the eighth element.
*/
public static Vector<N8> fill(double n1, double n2, double n3, double n4, double n5,
double n6, double n7, double n8) {
return new VecBuilder<>(Nat.N8()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8);
}
/**
* Returns a 9x1 vector containing the given elements.
*
* @param n1 the first element.
* @param n2 the second element.
* @param n3 the third element.
* @param n4 the fourth element.
* @param n5 the fifth element.
* @param n6 the sixth element.
* @param n7 the seventh element.
* @param n8 the eighth element.
* @param n9 the ninth element.
*/
public static Vector<N9> fill(double n1, double n2, double n3, double n4, double n5,
double n6, double n7, double n8, double n9) {
return new VecBuilder<>(Nat.N9()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9);
}
/**
* Returns a 10x1 vector containing the given elements.
*
* @param n1 the first element.
* @param n2 the second element.
* @param n3 the third element.
* @param n4 the fourth element.
* @param n5 the fifth element.
* @param n6 the sixth element.
* @param n7 the seventh element.
* @param n8 the eighth element.
* @param n9 the ninth element.
* @param n10 the tenth element.
*/
@SuppressWarnings("PMD.ExcessiveParameterList")
public static Vector<N10> fill(double n1, double n2, double n3, double n4, double n5,
double n6, double n7, double n8, double n9, double n10) {
return new VecBuilder<>(Nat.N10()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
}
}

55
wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java Normal file
View File

@@ -0,0 +1,55 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpiutil.math;
import org.ejml.simple.SimpleMatrix;
import edu.wpi.first.wpiutil.math.numbers.N1;
/**
* A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
*
* <p>This class is intended to be used alongside the state space library.
*
* @param <R> The number of rows in this matrix.
*/
public class Vector<R extends Num> extends Matrix<R, N1> {
/**
* Constructs an empty zero vector of the given dimensions.
*
* @param rows The number of rows of the vector.
*/
public Vector(Nat<R> rows) {
super(rows, Nat.N1());
}
/**
* Constructs a new {@link Vector} with the given storage.
* Caller should make sure that the provided generic bounds match
* the shape of the provided {@link Vector}.
*
* <p>NOTE:It is not recommended to use this constructor unless the
* {@link SimpleMatrix} API is absolutely necessary due to the desired
* function not being accessible through the {@link Vector} wrapper.
*
* @param storage The {@link SimpleMatrix} to back this vector.
*/
public Vector(SimpleMatrix storage) {
super(storage);
}
/**
* Constructs a new vector with the storage of the supplied matrix.
*
* @param other The {@link Vector} to copy the storage of.
*/
public Vector(Matrix<R, N1> other) {
super(other);
}
}

79
wpimath/src/main/java/edu/wpi/first/wpiutil/math/WPIMathJNI.java Normal file
View File

@@ -0,0 +1,79 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpiutil.math;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
import edu.wpi.first.wpiutil.RuntimeLoader;
public final class WPIMathJNI {
static boolean libraryLoaded = false;
static RuntimeLoader<WPIMathJNI> loader = null;
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
}
static {
if (Helper.getExtractOnStaticLoad()) {
try {
loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class);
loader.loadLibrary();
} catch (IOException ex) {
ex.printStackTrace();
System.exit(1);
}
libraryLoaded = true;
}
}
/**
* Force load the library.
*/
public static synchronized void forceLoad() throws IOException {
if (libraryLoaded) {
return;
}
loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class);
loader.loadLibrary();
libraryLoaded = true;
}
/**
* Computes the matrix exp.
*
* @param src Array of elements of the matrix to be exponentiated.
* @param rows how many rows there are.
* @param dst Array where the result will be stored.
*/
public static native void exp(double[] src, int rows, double[] dst);
/**
* Returns true if (A, B) is a stabilizable pair.
*
* <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
* any, have absolute values less than one, where an eigenvalue is
* uncontrollable if rank(lambda * I - A, B) &lt; n where n is number of states.
*
* @param states the number of states of the system.
* @param inputs the number of inputs to the system.
* @param A System matrix.
* @param B Input matrix.
* @return If the system is stabilizable.
*/
public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
}