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

[wpimath] Clean up VecBuilder and MatBuilder (#5906)

Browse Source
This commit is contained in:
Joseph Eng
2023-11-14 12:23:50 -08:00
committed by GitHub
parent e117274a67
commit 79dd795bc0
29 changed files with 369 additions and 380 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -14,8 +14,47 @@ import org.ejml.simple.SimpleMatrix;
* @param <C> The number of columns of the desired matrix.
*/
public class MatBuilder<R extends Num, C extends Num> {
final Nat<R> m_rows;
final Nat<C> m_cols;
/**
* Fills the matrix with the given data, encoded in row major form. (The matrix is filled row by
* row, left to right with the given data).
*
* @param rows The number of rows of the matrix.
* @param cols The number of columns of the matrix.
* @param data The data to fill the matrix with.
* @param <R> The number of rows of the matrix.
* @param <C> The number of columns of the matrix.
* @return The constructed matrix.
*/
public static final <R extends Num, C extends Num> Matrix<R, C> fill(
Nat<R> rows, Nat<C> cols, double... data) {
if (Objects.requireNonNull(data).length != rows.getNum() * cols.getNum()) {
throw new IllegalArgumentException(
"Invalid matrix data provided. Wanted "
+ rows.getNum()
+ " x "
+ cols.getNum()
+ " matrix, but got "
+ data.length
+ " elements");
}
return new Matrix<>(new SimpleMatrix(rows.getNum(), cols.getNum(), true, data));
}
protected final Nat<R> m_rows;
protected final Nat<C> m_cols;
/**
* Creates a new {@link MatBuilder} with the given dimensions.
*
* @param rows The number of rows of the matrix.
* @param cols The number of columns of the matrix.
* @deprecated Use {@link MatBuilder#fill} instead.
*/
@Deprecated(since = "2024", forRemoval = true)
public MatBuilder(Nat<R> rows, Nat<C> cols) {
this.m_rows = Objects.requireNonNull(rows);
this.m_cols = Objects.requireNonNull(cols);
}
/**
* Fills the matrix with the given data, encoded in row major form. (The matrix is filled row by
@@ -25,28 +64,6 @@ public class MatBuilder<R extends Num, C extends Num> {
* @return The constructed matrix.
*/
public final Matrix<R, C> fill(double... data) {
if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) {
throw new IllegalArgumentException(
"Invalid matrix data provided. Wanted "
+ this.m_rows.getNum()
+ " x "
+ this.m_cols.getNum()
+ " matrix, but got "
+ data.length
+ " elements");
} else {
return new Matrix<>(new SimpleMatrix(this.m_rows.getNum(), this.m_cols.getNum(), true, data));
}
}
/**
* Creates a new {@link MatBuilder} with the given dimensions.
*
* @param rows The number of rows of the matrix.
* @param cols The number of columns of the matrix.
*/
public MatBuilder(Nat<R> rows, Nat<C> cols) {
this.m_rows = Objects.requireNonNull(rows);
this.m_cols = Objects.requireNonNull(cols);
return fill(m_rows, m_cols, data);
}
}

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

@@ -654,7 +654,10 @@ public class Matrix<R extends Num, C extends Num> {
* @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.
* @deprecated Use {@link MatBuilder#fill} instead.
*/
@Deprecated(since = "2024", forRemoval = true)
@SuppressWarnings("removal")
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));
}

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

@@ -17,26 +17,22 @@ import edu.wpi.first.math.numbers.N9;
import java.util.Objects;
import org.ejml.simple.SimpleMatrix;
/**
* A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
*
* @param <N> The dimension of the vector to be constructed.
*/
public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
public VecBuilder(Nat<N> rows) {
super(rows, Nat.N1());
/** A class for constructing vectors (Nx1 matrices). */
public final class VecBuilder {
private VecBuilder() {
throw new UnsupportedOperationException("this is a utility class!");
}
private Vector<N> fillVec(double... data) {
if (Objects.requireNonNull(data).length != this.m_rows.getNum()) {
private static <N extends Num> Vector<N> fillVec(Nat<N> rows, double... data) {
if (Objects.requireNonNull(data).length != rows.getNum()) {
throw new IllegalArgumentException(
"Invalid vector data provided. Wanted "
+ this.m_rows.getNum()
+ rows.getNum()
+ " vector, but got "
+ data.length
+ " elements");
}
return new Vector<>(new SimpleMatrix(this.m_rows.getNum(), 1, true, data));
return new Vector<>(new SimpleMatrix(data));
}
/**
@@ -46,7 +42,7 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
* @return 1x1 vector
*/
public static Vector<N1> fill(double n1) {
return new VecBuilder<>(Nat.N1()).fillVec(n1);
return fillVec(Nat.N1(), n1);
}
/**
@@ -57,7 +53,7 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
* @return 2x1 vector
*/
public static Vector<N2> fill(double n1, double n2) {
return new VecBuilder<>(Nat.N2()).fillVec(n1, n2);
return fillVec(Nat.N2(), n1, n2);
}
/**
@@ -69,7 +65,7 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
* @return 3x1 vector
*/
public static Vector<N3> fill(double n1, double n2, double n3) {
return new VecBuilder<>(Nat.N3()).fillVec(n1, n2, n3);
return fillVec(Nat.N3(), n1, n2, n3);
}
/**
@@ -82,7 +78,7 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
* @return 4x1 vector
*/
public static Vector<N4> fill(double n1, double n2, double n3, double n4) {
return new VecBuilder<>(Nat.N4()).fillVec(n1, n2, n3, n4);
return fillVec(Nat.N4(), n1, n2, n3, n4);
}
/**
@@ -96,7 +92,7 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
* @return 5x1 vector
*/
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);
return fillVec(Nat.N5(), n1, n2, n3, n4, n5);
}
/**
@@ -111,7 +107,7 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
* @return 6x1 vector
*/
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);
return fillVec(Nat.N6(), n1, n2, n3, n4, n5, n6);
}
/**
@@ -128,7 +124,7 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
*/
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);
return fillVec(Nat.N7(), n1, n2, n3, n4, n5, n6, n7);
}
/**
@@ -146,7 +142,7 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
*/
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);
return fillVec(Nat.N8(), n1, n2, n3, n4, n5, n6, n7, n8);
}
/**
@@ -173,7 +169,7 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
double n7,
double n8,
double n9) {
return new VecBuilder<>(Nat.N9()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9);
return fillVec(Nat.N9(), n1, n2, n3, n4, n5, n6, n7, n8, n9);
}
/**
@@ -202,6 +198,6 @@ public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
double n8,
double n9,
double n10) {
return new VecBuilder<>(Nat.N10()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
return fillVec(Nat.N10(), n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
}
}

11
wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java
View File

@@ -5,7 +5,10 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
@@ -80,10 +83,10 @@ public class DifferentialDriveAccelerationLimiter {
*/
public DifferentialDriveWheelVoltages calculate(
double leftVelocity, double rightVelocity, double leftVoltage, double rightVoltage) {
var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVoltage, rightVoltage);
Matrix<N2, N1> u = VecBuilder.fill(leftVoltage, rightVoltage);
// Find unconstrained wheel accelerations
var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVelocity, rightVelocity);
var x = VecBuilder.fill(leftVelocity, rightVelocity);
var dxdt = m_system.getA().times(x).plus(m_system.getB().times(u));
// Convert from wheel accelerations to linear and angular accelerations
@@ -98,9 +101,7 @@ public class DifferentialDriveAccelerationLimiter {
// [α] [-1/trackwidth 1/trackwidth][dxdt(1)]
//
// accels = M dxdt where M = [0.5, 0.5; -1/trackwidth, 1/trackwidth]
var M =
new MatBuilder<>(Nat.N2(), Nat.N2())
.fill(0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth);
var M = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth);
var accels = M.times(dxdt);
// Constrain the linear and angular accelerations

6
wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.system.plant.LinearSystemId;
@@ -93,8 +93,8 @@ public class ElevatorFeedforward {
var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity);
var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity);
return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
}

126
wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
View File

@@ -11,6 +11,7 @@ import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.StateSpaceUtil;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.numbers.N1;
@@ -88,46 +89,48 @@ public class LTVDifferentialDriveController {
// Control law derivation is in section 8.7 of
// https://file.tavsys.net/control/controls-engineering-in-frc.pdf
var A =
new MatBuilder<>(Nat.N5(), Nat.N5())
.fill(
0.0,
0.0,
0.0,
0.5,
0.5,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
-1.0 / m_trackwidth,
1.0 / m_trackwidth,
0.0,
0.0,
0.0,
plant.getA(0, 0),
plant.getA(0, 1),
0.0,
0.0,
0.0,
plant.getA(1, 0),
plant.getA(1, 1));
MatBuilder.fill(
Nat.N5(),
Nat.N5(),
0.0,
0.0,
0.0,
0.5,
0.5,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
-1.0 / m_trackwidth,
1.0 / m_trackwidth,
0.0,
0.0,
0.0,
plant.getA(0, 0),
plant.getA(0, 1),
0.0,
0.0,
0.0,
plant.getA(1, 0),
plant.getA(1, 1));
var B =
new MatBuilder<>(Nat.N5(), Nat.N2())
.fill(
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
plant.getB(0, 0),
plant.getB(0, 1),
plant.getB(1, 0),
plant.getB(1, 1));
MatBuilder.fill(
Nat.N5(),
Nat.N2(),
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
plant.getB(0, 0),
plant.getB(0, 1),
plant.getB(1, 0),
plant.getB(1, 1));
var Q = StateSpaceUtil.makeCostMatrix(qelems);
var R = StateSpaceUtil.makeCostMatrix(relems);
@@ -136,11 +139,7 @@ public class LTVDifferentialDriveController {
// Ax = -Bu
// x = -A⁻¹Bu
double maxV =
plant
.getA()
.solve(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)))
.times(-1.0)
.get(0, 0);
plant.getA().solve(plant.getB().times(VecBuilder.fill(12.0, 12.0))).times(-1.0).get(0, 0);
if (maxV <= 0.0) {
throw new IllegalArgumentException(
@@ -201,13 +200,12 @@ public class LTVDifferentialDriveController {
public void setTolerance(
Pose2d poseTolerance, double leftVelocityTolerance, double rightVelocityTolerance) {
m_tolerance =
new MatBuilder<>(Nat.N5(), Nat.N1())
.fill(
poseTolerance.getX(),
poseTolerance.getY(),
poseTolerance.getRotation().getRadians(),
leftVelocityTolerance,
rightVelocityTolerance);
VecBuilder.fill(
poseTolerance.getX(),
poseTolerance.getY(),
poseTolerance.getRotation().getRadians(),
leftVelocityTolerance,
rightVelocityTolerance);
}
/**
@@ -234,13 +232,12 @@ public class LTVDifferentialDriveController {
// This implements the linear time-varying differential drive controller in
// theorem 9.6.3 of https://tavsys.net/controls-in-frc.
var x =
new MatBuilder<>(Nat.N5(), Nat.N1())
.fill(
currentPose.getX(),
currentPose.getY(),
currentPose.getRotation().getRadians(),
leftVelocity,
rightVelocity);
VecBuilder.fill(
currentPose.getX(),
currentPose.getY(),
currentPose.getRotation().getRadians(),
leftVelocity,
rightVelocity);
var inRobotFrame = Matrix.eye(Nat.N5());
inRobotFrame.set(0, 0, Math.cos(x.get(State.kHeading.value, 0)));
@@ -249,13 +246,12 @@ public class LTVDifferentialDriveController {
inRobotFrame.set(1, 1, Math.cos(x.get(State.kHeading.value, 0)));
var r =
new MatBuilder<>(Nat.N5(), Nat.N1())
.fill(
poseRef.getX(),
poseRef.getY(),
poseRef.getRotation().getRadians(),
leftVelocityRef,
rightVelocityRef);
VecBuilder.fill(
poseRef.getX(),
poseRef.getY(),
poseRef.getRotation().getRadians(),
leftVelocityRef,
rightVelocityRef);
m_error = r.minus(x);
m_error.set(
State.kHeading.value, 0, MathUtil.angleModulus(m_error.get(State.kHeading.value, 0)));

10
wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
View File

@@ -149,7 +149,7 @@ public class LTVUnicycleController {
// A = [0 0 v] B = [0 0]
// [0 0 0] [0 1]
var A = new Matrix<>(Nat.N3(), Nat.N3());
var B = new MatBuilder<>(Nat.N3(), Nat.N2()).fill(1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
var B = MatBuilder.fill(Nat.N3(), Nat.N2(), 1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
var Q = StateSpaceUtil.makeCostMatrix(qelems);
var R = StateSpaceUtil.makeCostMatrix(relems);
@@ -226,8 +226,12 @@ public class LTVUnicycleController {
var K = m_table.get(linearVelocityRef);
var e =
new MatBuilder<>(Nat.N3(), Nat.N1())
.fill(m_poseError.getX(), m_poseError.getY(), m_poseError.getRotation().getRadians());
MatBuilder.fill(
Nat.N3(),
Nat.N1(),
m_poseError.getX(),
m_poseError.getY(),
m_poseError.getRotation().getRadians());
var u = K.times(e);
return new ChassisSpeeds(

6
wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.system.plant.LinearSystemId;
@@ -62,8 +62,8 @@ public class SimpleMotorFeedforward {
var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity);
var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity);
return ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
}

5
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -8,7 +8,6 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
@@ -201,9 +200,7 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
// so a 180 degree rotation is required. Any orthogonal vector can be used
// for it. Q in the QR decomposition is an orthonormal basis, so it
// contains orthogonal unit vectors.
var X =
new MatBuilder<>(Nat.N3(), Nat.N1())
.fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0));
var X = VecBuilder.fill(initial.get(0, 0), initial.get(1, 0), initial.get(2, 0));
final var qr = DecompositionFactory_DDRM.qr(3, 1);
qr.decompose(X.getStorage().getMatrix());
final var Q = qr.getQ(null, false);

84
wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math.system.plant;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
@@ -40,20 +41,17 @@ public final class LinearSystemId {
}
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2())
.fill(
0,
1,
0,
-Math.pow(gearing, 2)
* motor.KtNMPerAmp
/ (motor.rOhms
* radiusMeters
* radiusMeters
* massKg
* motor.KvRadPerSecPerVolt)),
MatBuilder.fill(
Nat.N2(),
Nat.N2(),
0,
1,
0,
-Math.pow(gearing, 2)
* motor.KtNMPerAmp
/ (motor.rOhms * radiusMeters * radiusMeters * massKg * motor.KvRadPerSecPerVolt)),
VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)),
Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0),
new Matrix<>(Nat.N1(), Nat.N1()));
}
@@ -108,15 +106,16 @@ public final class LinearSystemId {
}
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2())
.fill(
0,
1,
0,
-gearing
* gearing
* motor.KtNMPerAmp
/ (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)),
MatBuilder.fill(
Nat.N2(),
Nat.N2(),
0,
1,
0,
-gearing
* gearing
* motor.KtNMPerAmp
/ (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)),
VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
@@ -149,7 +148,7 @@ public final class LinearSystemId {
}
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kV / kA),
MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kV / kA),
VecBuilder.fill(0, 1 / kA),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
@@ -201,10 +200,10 @@ public final class LinearSystemId {
final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared;
final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared;
var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C1, C4 * C1, C4 * C1, C3 * C1);
var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C2, C4 * C2, C4 * C2, C3 * C2);
var C = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0);
var D = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0);
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1);
var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2);
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0);
return new LinearSystem<>(A, B, C, D);
}
@@ -229,16 +228,17 @@ public final class LinearSystemId {
}
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2())
.fill(
0,
1,
0,
-Math.pow(gearing, 2)
* motor.KtNMPerAmp
/ (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)),
MatBuilder.fill(
Nat.N2(),
Nat.N2(),
0,
1,
0,
-Math.pow(gearing, 2)
* motor.KtNMPerAmp
/ (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)),
VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)),
Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0),
new Matrix<>(Nat.N1(), Nat.N1()));
}
@@ -302,9 +302,9 @@ public final class LinearSystemId {
}
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA),
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA),
VecBuilder.fill(0.0, 1.0 / kA),
Matrix.mat(Nat.N1(), Nat.N2()).fill(1.0, 0.0),
MatBuilder.fill(Nat.N1(), Nat.N2(), 1.0, 0.0),
VecBuilder.fill(0.0));
}
@@ -347,10 +347,10 @@ public final class LinearSystemId {
final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular);
return new LinearSystem<>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1),
Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0));
MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1),
MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1),
MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1),
MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0));
}
/**