Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Reorganize LinearSystem factories (#8468)

Browse Source
This commit is contained in:
Tyler Veness
2026-01-12 19:09:35 -08:00
committed by GitHub
parent 89d0759ef2
commit 00fa8361dd
108 changed files with 1808 additions and 2138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveAccelerationLimiterTest.java
View File

@@ -14,7 +14,7 @@ import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.system.Models;
class DifferentialDriveAccelerationLimiterTest {
@Test
@@ -24,7 +24,7 @@ class DifferentialDriveAccelerationLimiterTest {
final double maxA = 2.0;
final double maxAlpha = 2.0;
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, maxA, maxAlpha);
@@ -114,7 +114,7 @@ class DifferentialDriveAccelerationLimiterTest {
final double trackwidth = 0.9;
final double dt = 0.005;
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
// Limits are so high, they don't get hit, so states of constrained and
// unconstrained systems should match
@@ -178,7 +178,7 @@ class DifferentialDriveAccelerationLimiterTest {
final double maxA = 2.0;
final double maxAlpha = 2.0;
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
var accelLimiter =
new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha);
@@ -237,7 +237,7 @@ class DifferentialDriveAccelerationLimiterTest {
@Test
void testMinAccelGreaterThanMaxAccel() {
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
assertDoesNotThrow(() -> new DifferentialDriveAccelerationLimiter(plant, 1, -1, 1, 1e3));
assertThrows(
IllegalArgumentException.class,

7
wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveFeedforwardTest.java
View File

@@ -12,7 +12,7 @@ import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.system.LinearSystem;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.system.Models;
class DifferentialDriveFeedforwardTest {
private static final double kVLinear = 1.0;
@@ -27,8 +27,7 @@ class DifferentialDriveFeedforwardTest {
DifferentialDriveFeedforward differentialDriveFeedforward =
new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
LinearSystem<N2, N2, N2> plant =
LinearSystemId.identifyDrivetrainSystem(
kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
Models.differentialDriveFromSysId(kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {
@@ -58,7 +57,7 @@ class DifferentialDriveFeedforwardTest {
DifferentialDriveFeedforward differentialDriveFeedforward =
new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular);
LinearSystem<N2, N2, N2> plant =
LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
Models.differentialDriveFromSysId(kVLinear, kALinear, kVAngular, kAAngular);
for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {

8
wpimath/src/test/java/org/wpilib/math/controller/ImplicitModelFollowerTest.java
View File

@@ -12,14 +12,14 @@ import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.system.Models;
class ImplicitModelFollowerTest {
@Test
void testSameModel() {
final double dt = 0.005;
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
var imf = new ImplicitModelFollower<>(plant, plant);
@@ -61,10 +61,10 @@ class ImplicitModelFollowerTest {
void testSlowerRefModel() {
final double dt = 0.005;
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
// Linear acceleration is slower, but angular acceleration is the same
var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0);
var plantRef = Models.differentialDriveFromSysId(1.0, 2.0, 1.0, 1.0);
var imf = new ImplicitModelFollower<>(plant, plantRef);

4
wpimath/src/test/java/org/wpilib/math/controller/LTVDifferentialDriveControllerTest.java
View File

@@ -18,8 +18,8 @@ import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.numbers.N5;
import org.wpilib.math.system.LinearSystem;
import org.wpilib.math.system.Models;
import org.wpilib.math.system.NumericalIntegration;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.trajectory.TrajectoryConfig;
import org.wpilib.math.trajectory.TrajectoryGenerator;
import org.wpilib.math.util.MathUtil;
@@ -52,7 +52,7 @@ class LTVDifferentialDriveControllerTest {
private static final double kAngularV = 1.382; // V/(m/s)
private static final double kAngularA = 0.08495; // V/(m/s²)
private static final LinearSystem<N2, N2, N2> plant =
LinearSystemId.identifyDrivetrainSystem(kLinearV, kLinearA, kAngularV, kAngularA);
Models.differentialDriveFromSysId(kLinearV, kLinearA, kAngularV, kAngularA);
private static final double kTrackwidth = 0.9;
private static Matrix<N5, N1> dynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {

12
wpimath/src/test/java/org/wpilib/math/controller/LinearQuadraticRegulatorTest.java
View File

@@ -10,9 +10,9 @@ import org.junit.jupiter.api.Test;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.system.DCMotor;
import org.wpilib.math.system.Discretization;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.system.Models;
import org.wpilib.math.util.Nat;
import org.wpilib.math.util.Num;
@@ -25,7 +25,7 @@ class LinearQuadraticRegulatorTest {
var r = 0.0181864;
var G = 1.0;
var plant = LinearSystemId.createElevatorSystem(motors, m, r, G);
var plant = Models.elevatorFromPhysicalConstants(motors, m, r, G);
var qElms = VecBuilder.fill(0.02, 0.4);
var rElms = VecBuilder.fill(12.0);
@@ -42,7 +42,7 @@ class LinearQuadraticRegulatorTest {
var dt = 0.020;
var plant =
LinearSystemId.createElevatorSystem(
Models.elevatorFromPhysicalConstants(
DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
var K =
@@ -61,7 +61,7 @@ class LinearQuadraticRegulatorTest {
var r = 0.4;
var G = 100.0;
var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G);
var plant = Models.singleJointedArmFromPhysicalConstants(motors, 1d / 3d * m * r * r, G);
var qElms = VecBuilder.fill(0.01745, 0.08726);
var rElms = VecBuilder.fill(12.0);
@@ -160,7 +160,7 @@ class LinearQuadraticRegulatorTest {
var dt = 0.02;
var plant =
LinearSystemId.createElevatorSystem(
Models.elevatorFromPhysicalConstants(
DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
var regulator =

8
wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java
View File

@@ -14,10 +14,10 @@ import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.system.DCMotor;
import org.wpilib.math.system.LinearSystem;
import org.wpilib.math.system.LinearSystemLoop;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.system.Models;
import org.wpilib.math.trajectory.TrapezoidProfile;
import org.wpilib.math.util.Nat;
@@ -27,7 +27,7 @@ class LinearSystemLoopTest {
private static final Random random = new Random();
LinearSystem<N2, N1, N2> m_plant =
LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0);
Models.elevatorFromPhysicalConstants(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0);
@SuppressWarnings("unchecked")
KalmanFilter<N2, N1, N1> m_observer =
@@ -95,7 +95,7 @@ class LinearSystemLoopTest {
@Test
void testFlywheelEnabled() {
LinearSystem<N1, N1, N1> plant =
LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0);
Models.flywheelFromPhysicalConstants(DCMotor.getNEO(2), 0.00289, 1.0);
KalmanFilter<N1, N1, N1> observer =
new KalmanFilter<>(
Nat.N1(), Nat.N1(), plant, VecBuilder.fill(1.0), VecBuilder.fill(kPositionStddev), kDt);

2
wpimath/src/test/java/org/wpilib/math/estimator/ExtendedKalmanFilterTest.java
View File

@@ -18,9 +18,9 @@ import org.wpilib.math.numbers.N2;
import org.wpilib.math.numbers.N3;
import org.wpilib.math.numbers.N5;
import org.wpilib.math.random.Normal;
import org.wpilib.math.system.DCMotor;
import org.wpilib.math.system.NumericalIntegration;
import org.wpilib.math.system.NumericalJacobian;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.trajectory.TrajectoryConfig;
import org.wpilib.math.trajectory.TrajectoryGenerator;
import org.wpilib.math.util.Nat;

6
wpimath/src/test/java/org/wpilib/math/estimator/KalmanFilterTest.java
View File

@@ -19,9 +19,9 @@ import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N2;
import org.wpilib.math.numbers.N3;
import org.wpilib.math.numbers.N6;
import org.wpilib.math.system.DCMotor;
import org.wpilib.math.system.LinearSystem;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.system.Models;
import org.wpilib.math.trajectory.TrajectoryConfig;
import org.wpilib.math.trajectory.TrajectoryGenerator;
import org.wpilib.math.util.Nat;
@@ -41,7 +41,7 @@ class KalmanFilterTest {
var m = 5.0;
var r = 0.0181864;
var G = 1.0;
elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
elevatorPlant = Models.elevatorFromPhysicalConstants(motors, m, r, G);
}
// A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]ᵀ,

6
wpimath/src/test/java/org/wpilib/math/estimator/MerweUKFTest.java
View File

@@ -23,11 +23,11 @@ import org.wpilib.math.numbers.N3;
import org.wpilib.math.numbers.N4;
import org.wpilib.math.numbers.N5;
import org.wpilib.math.random.Normal;
import org.wpilib.math.system.DCMotor;
import org.wpilib.math.system.Discretization;
import org.wpilib.math.system.Models;
import org.wpilib.math.system.NumericalIntegration;
import org.wpilib.math.system.NumericalJacobian;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.trajectory.TrajectoryConfig;
import org.wpilib.math.trajectory.TrajectoryGenerator;
import org.wpilib.math.util.Nat;
@@ -222,7 +222,7 @@ class MerweUKFTest {
@Test
void testLinearUKF() {
var dt = 0.020;
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
var plant = Models.flywheelFromSysId(0.02, 0.006);
var observer =
new MerweUKF<>(
Nat.N1(),

6
wpimath/src/test/java/org/wpilib/math/estimator/S3UKFTest.java
View File

@@ -23,11 +23,11 @@ import org.wpilib.math.numbers.N3;
import org.wpilib.math.numbers.N4;
import org.wpilib.math.numbers.N5;
import org.wpilib.math.random.Normal;
import org.wpilib.math.system.DCMotor;
import org.wpilib.math.system.Discretization;
import org.wpilib.math.system.Models;
import org.wpilib.math.system.NumericalIntegration;
import org.wpilib.math.system.NumericalJacobian;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.trajectory.TrajectoryConfig;
import org.wpilib.math.trajectory.TrajectoryGenerator;
import org.wpilib.math.util.Nat;
@@ -222,7 +222,7 @@ class S3UKFTest {
@Test
void testLinearUKF() {
var dt = 0.020;
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
var plant = Models.flywheelFromSysId(0.02, 0.006);
var observer =
new S3UKF<>(
Nat.N1(),

79
wpimath/src/test/java/org/wpilib/math/system/LinearSystemIDTest.java → wpimath/src/test/java/org/wpilib/math/system/ModelsTest.java
View File

@@ -9,17 +9,36 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
import org.junit.jupiter.api.Test;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.system.plant.LinearSystemId;
import org.wpilib.math.util.Nat;
class LinearSystemIDTest {
class ModelsTest {
@Test
void testDrivetrainVelocitySystem() {
void testFlywheelFromPhysicalConstants() {
var model = Models.flywheelFromPhysicalConstants(DCMotor.getNEO(2), 0.00032, 1.0);
assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001));
assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001));
assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001));
assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
}
@Test
void testFlywheelFromSysId() {
var kv = 1.0;
var ka = 0.5;
var model = Models.flywheelFromSysId(kv, ka);
assertEquals(model.getA(), VecBuilder.fill(-kv / ka));
assertEquals(model.getB(), VecBuilder.fill(1 / ka));
}
@Test
void testDifferentialDriveFromPhysicalConstants() {
var model =
LinearSystemId.createDrivetrainVelocitySystem(DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6);
Models.differentialDriveFromPhysicalConstants(DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6);
assertTrue(
model
.getA()
@@ -41,8 +60,8 @@ class LinearSystemIDTest {
}
@Test
void testElevatorSystem() {
var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12);
void testElevatorFromPhysicalConstants() {
var model = Models.elevatorFromPhysicalConstants(DCMotor.getNEO(2), 5, 0.05, 12);
assertTrue(
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -99.05473), 0.001));
@@ -54,52 +73,24 @@ class LinearSystemIDTest {
}
@Test
void testFlywheelSystem() {
var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0);
assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001));
assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001));
assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001));
assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
}
@Test
void testDCMotorSystem() {
var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0);
assertTrue(
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -26.87032), 0.001));
assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001));
assertTrue(model.getC().isEqual(Matrix.eye(Nat.N2()), 0.001));
assertTrue(model.getD().isEqual(new Matrix<>(Nat.N2(), Nat.N1()), 0.001));
}
@Test
void testIdentifyPositionSystem() {
// By controls engineering in frc,
// x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
void testElevatorFromSysId() {
var kv = 1.0;
var ka = 0.5;
var model = LinearSystemId.identifyPositionSystem(kv, ka);
var model = Models.elevatorFromSysId(kv, ka);
assertEquals(model.getA(), MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kv / ka));
assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka));
}
@Test
void testIdentifyVelocitySystem() {
// By controls engineering in frc,
// V = kv * velocity + ka * acceleration
// x-dot = -kv/ka * v + 1/ka \cdot V
void testSingleJointedArmFromSysId() {
var kv = 1.0;
var ka = 0.5;
var model = LinearSystemId.identifyVelocitySystem(kv, ka);
assertEquals(model.getA(), VecBuilder.fill(-kv / ka));
assertEquals(model.getB(), VecBuilder.fill(1 / ka));
var model = Models.singleJointedArmFromSysId(kv, ka);
assertTrue(
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kv / ka), 0.001));
assertTrue(model.getB().isEqual(MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 1.0 / ka), 0.001));
}
}

6
wpimath/src/test/java/org/wpilib/math/system/plant/proto/DCMotorProtoTest.java → wpimath/src/test/java/org/wpilib/math/system/proto/DCMotorProtoTest.java
View File

@@ -2,13 +2,13 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.system.plant.proto;
package org.wpilib.math.system.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
import org.wpilib.math.proto.Plant.ProtobufDCMotor;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.proto.System.ProtobufDCMotor;
import org.wpilib.math.system.DCMotor;
class DCMotorProtoTest {
private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3);

4
wpimath/src/test/java/org/wpilib/math/system/plant/struct/DCMotorStructTest.java → wpimath/src/test/java/org/wpilib/math/system/struct/DCMotorStructTest.java
View File

@@ -2,14 +2,14 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.system.plant.struct;
package org.wpilib.math.system.struct;
import static org.junit.jupiter.api.Assertions.assertEquals;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
import org.wpilib.math.system.plant.DCMotor;
import org.wpilib.math.system.DCMotor;
class DCMotorStructTest {
private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3);

7
wpimath/src/test/native/cpp/StateSpaceTest.cpp
View File

@@ -11,10 +11,10 @@
#include "wpi/math/controller/LinearQuadraticRegulator.hpp"
#include "wpi/math/estimator/KalmanFilter.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/system/DCMotor.hpp"
#include "wpi/math/system/LinearSystem.hpp"
#include "wpi/math/system/LinearSystemLoop.hpp"
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/system/Models.hpp"
#include "wpi/units/time.hpp"
namespace wpi::math {
@@ -36,7 +36,8 @@ class StateSpaceTest : public testing::Test {
// Gear ratio
constexpr double G = 40.0 / 40.0;
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G)
.Slice(0);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.02, 0.4}, {12.0}, kDt};
KalmanFilter<2, 1, 1> observer{plant, {0.05, 1.0}, {0.0001}, kDt};

18
wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp
View File

@@ -6,7 +6,7 @@
#include <gtest/gtest.h>
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/system/Models.hpp"
#include "wpi/units/math.hpp"
namespace wpi::math {
@@ -19,8 +19,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, maxA,
maxAlpha};
@@ -111,8 +111,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
// Limits are so high, they don't get hit, so states of constrained and
// unconstrained systems should match
@@ -181,8 +181,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, minA,
maxA, maxAlpha};
@@ -240,8 +240,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
TEST(DifferentialDriveAccelerationLimiterTest, MinAccelGreaterThanMaxAccel) {
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
EXPECT_NO_THROW({
DifferentialDriveAccelerationLimiter accelLimiter(plant, 1_m, 1_mps_sq,
1_rad_per_s_sq);

11
wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp
View File

@@ -4,13 +4,10 @@
#include "wpi/math/controller/DifferentialDriveFeedforward.hpp"
#include <cmath>
#include <Eigen/Core>
#include <gtest/gtest.h>
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/system/Models.hpp"
#include "wpi/units/acceleration.hpp"
#include "wpi/units/length.hpp"
#include "wpi/units/time.hpp"
@@ -26,7 +23,7 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithTrackwidth) {
wpi::math::DifferentialDriveFeedforward differentialDriveFeedforward{
kVLinear, kALinear, kVAngular, kAAngular, trackwidth};
wpi::math::LinearSystem<2, 2, 2> plant =
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
wpi::math::Models::DifferentialDriveFromSysId(
kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
currentLeftVelocity += 2_mps) {
@@ -60,8 +57,8 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) {
wpi::math::DifferentialDriveFeedforward differentialDriveFeedforward{
kVLinear, kALinear, kVAngular, kAAngular};
wpi::math::LinearSystem<2, 2, 2> plant =
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear,
kVAngular, kAAngular);
wpi::math::Models::DifferentialDriveFromSysId(kVLinear, kALinear,
kVAngular, kAAngular);
for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
currentLeftVelocity += 2_mps) {
for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps;

4
wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp
View File

@@ -4,15 +4,13 @@
#include "wpi/math/controller/ElevatorFeedforward.hpp"
#include <cmath>
#include <gtest/gtest.h>
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/units/acceleration.hpp"
#include "wpi/units/length.hpp"
#include "wpi/units/time.hpp"
#include "wpi/units/velocity.hpp"
static constexpr auto Ks = 0.5_V;
static constexpr auto Kv = 1.5_V * 1_s / 1_m;

14
wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp
View File

@@ -6,7 +6,7 @@
#include <gtest/gtest.h>
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/system/Models.hpp"
namespace wpi::math {
@@ -15,8 +15,8 @@ TEST(ImplicitModelFollowerTest, SameModel) {
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
ImplicitModelFollower<2, 2> imf{plant, plant};
@@ -60,12 +60,12 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) {
using Kv_t = decltype(1_V / 1_mps);
using Ka_t = decltype(1_V / 1_mps_sq);
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
Kv_t{1.0}, Ka_t{1.0});
// Linear acceleration is slower, but angular acceleration is the same
auto plantRef = LinearSystemId::IdentifyDrivetrainSystem(
Kv_t{1.0}, Ka_t{2.0}, Kv_t{1.0}, Ka_t{1.0});
auto plantRef = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{2.0},
Kv_t{1.0}, Ka_t{1.0});
ImplicitModelFollower<2, 2> imf{plant, plantRef};

4
wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp
View File

@@ -8,8 +8,8 @@
#include <gtest/gtest.h>
#include "wpi/math/system/Models.hpp"
#include "wpi/math/system/NumericalIntegration.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/math/util/MathUtil.hpp"
#include "wpi/units/math.hpp"
@@ -46,7 +46,7 @@ static constexpr auto kLinearV = 3.02_V / 1_mps;
static constexpr auto kLinearA = 0.642_V / 1_mps_sq;
static constexpr auto kAngularV = 1.382_V / 1_mps;
static constexpr auto kAngularA = 0.08495_V / 1_mps_sq;
static auto plant = wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
static auto plant = wpi::math::Models::DifferentialDriveFromSysId(
kLinearV, kLinearA, kAngularV, kAngularA);
static constexpr auto kTrackwidth = 0.9_m;

2
wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
View File

@@ -4,8 +4,6 @@
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"
#include <cmath>
#include <gtest/gtest.h>
#include "wpi/math/linalg/EigenCore.hpp"

17
wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
View File

@@ -4,14 +4,12 @@
#include "wpi/math/controller/LinearQuadraticRegulator.hpp"
#include <cmath>
#include <gtest/gtest.h>
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/system/DCMotor.hpp"
#include "wpi/math/system/LinearSystem.hpp"
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/system/Models.hpp"
#include "wpi/units/time.hpp"
namespace wpi::math {
@@ -29,7 +27,8 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
// Gear ratio
constexpr double G = 40.0 / 40.0;
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G)
.Slice(0);
}();
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5_ms}.K();
@@ -51,7 +50,7 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
// Gear ratio
constexpr double G = 100.0;
return wpi::math::LinearSystemId::SingleJointedArmSystem(
return wpi::math::Models::SingleJointedArmFromPhysicalConstants(
motors, 1.0 / 3.0 * m * r * r, G)
.Slice(0);
}();
@@ -77,7 +76,8 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
// Gear ratio
constexpr double G = 14.67;
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G)
.Slice(0);
}();
Matrixd<1, 2> K =
LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K();
@@ -179,7 +179,8 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
// Gear ratio
constexpr double G = 14.67;
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G)
.Slice(0);
}();
LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 20_ms};

2
wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
View File

@@ -4,8 +4,6 @@
#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
#include <cmath>
#include <gtest/gtest.h>
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"

1
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp
View File

@@ -16,7 +16,6 @@
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/units/angle.hpp"
#include "wpi/units/length.hpp"
#include "wpi/units/time.hpp"

1
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -17,7 +17,6 @@
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/math/util/StateSpaceUtil.hpp"
#include "wpi/units/angle.hpp"
#include "wpi/units/length.hpp"
#include "wpi/units/time.hpp"

2
wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
View File

@@ -12,8 +12,8 @@
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/random/Normal.hpp"
#include "wpi/math/system/DCMotor.hpp"
#include "wpi/math/system/NumericalJacobian.hpp"
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/units/moment_of_inertia.hpp"

6
wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
View File

@@ -10,14 +10,14 @@
#include <Eigen/Core>
#include <gtest/gtest.h>
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/system/DCMotor.hpp"
#include "wpi/math/system/Models.hpp"
#include "wpi/units/moment_of_inertia.hpp"
#include "wpi/units/time.hpp"
TEST(KalmanFilterTest, Flywheel) {
auto motor = wpi::math::DCMotor::NEO();
auto flywheel =
wpi::math::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0);
wpi::math::Models::FlywheelFromPhysicalConstants(motor, 1_kg_sq_m, 1.0);
wpi::math::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms};
}

9
wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp
View File

@@ -15,11 +15,11 @@
#include "wpi/math/estimator/AngleStatistics.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/random/Normal.hpp"
#include "wpi/math/system/DCMotor.hpp"
#include "wpi/math/system/Discretization.hpp"
#include "wpi/math/system/Models.hpp"
#include "wpi/math/system/NumericalIntegration.hpp"
#include "wpi/math/system/NumericalJacobian.hpp"
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/units/moment_of_inertia.hpp"
@@ -183,9 +183,8 @@ TEST(MerweUKFTest, DriveConvergence) {
TEST(MerweUKFTest, LinearUKF) {
constexpr wpi::units::second_t dt = 20_ms;
auto plant =
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
auto plant = wpi::math::Models::FlywheelFromSysId(0.02_V / 1_rad_per_s,
0.006_V / 1_rad_per_s_sq);
wpi::math::MerweUKF<1, 1, 1> observer{
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;

9
wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp
View File

@@ -15,11 +15,11 @@
#include "wpi/math/estimator/AngleStatistics.hpp"
#include "wpi/math/linalg/EigenCore.hpp"
#include "wpi/math/random/Normal.hpp"
#include "wpi/math/system/DCMotor.hpp"
#include "wpi/math/system/Discretization.hpp"
#include "wpi/math/system/Models.hpp"
#include "wpi/math/system/NumericalIntegration.hpp"
#include "wpi/math/system/NumericalJacobian.hpp"
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/units/moment_of_inertia.hpp"
@@ -183,9 +183,8 @@ TEST(S3UKFTest, DriveConvergence) {
TEST(S3UKFTest, LinearUKF) {
constexpr wpi::units::second_t dt = 20_ms;
auto plant =
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
auto plant = wpi::math::Models::FlywheelFromSysId(0.02_V / 1_rad_per_s,
0.006_V / 1_rad_per_s_sq);
wpi::math::S3UKF<1, 1, 1> observer{
[&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;

1
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
View File

@@ -16,7 +16,6 @@
#include "wpi/math/kinematics/SwerveDriveKinematics.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/util/print.hpp"
#include "wpi/util/timestamp.h"
void testFollowTrajectory(
const wpi::math::SwerveDriveKinematics<4>& kinematics,

1
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -17,7 +17,6 @@
#include "wpi/math/kinematics/SwerveDriveKinematics.hpp"
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
#include "wpi/util/print.hpp"
#include "wpi/util/timestamp.h"
void testFollowTrajectory(
const wpi::math::SwerveDriveKinematics<4>& kinematics,

116
wpimath/src/test/native/cpp/system/LinearSystemIdTest.cpp → wpimath/src/test/native/cpp/system/ModelsTest.cpp
View File

@@ -2,22 +2,54 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/system/plant/LinearSystemId.hpp"
#include "wpi/math/system/Models.hpp"
#include <gtest/gtest.h>
#include "wpi/math/system/DCMotor.hpp"
#include "wpi/math/system/LinearSystem.hpp"
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/units/length.hpp"
#include "wpi/units/mass.hpp"
TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
TEST(ModelsTest, FlywheelFromPhysicalConstants) {
#if __GNUC__ <= 11
auto model = wpi::math::LinearSystemId::DrivetrainVelocitySystem(
auto model = wpi::math::Models::FlywheelFromPhysicalConstants(
wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#else
constexpr auto model = wpi::math::Models::FlywheelFromPhysicalConstants(
wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#endif
ASSERT_TRUE(model.A().isApprox(wpi::math::Matrixd<1, 1>{-26.87032}, 0.001));
ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<1, 1>{1354.166667}, 0.001));
ASSERT_TRUE(model.C().isApprox(wpi::math::Matrixd<1, 1>{1.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<1, 1>{0.0}, 0.001));
}
TEST(ModelsTest, FlywheelFromSysId) {
constexpr double kv = 1.0;
constexpr double ka = 0.5;
#if __GNUC__ <= 11
auto model = wpi::math::Models::FlywheelFromSysId(kv * 1_V / 1_rad_per_s,
ka * 1_V / 1_rad_per_s_sq);
#else
constexpr auto model = wpi::math::Models::FlywheelFromSysId(
kv * 1_V / 1_rad_per_s, ka * 1_V / 1_rad_per_s_sq);
#endif
ASSERT_TRUE(model.A().isApprox(wpi::math::Matrixd<1, 1>{-kv / ka}, 0.001));
ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<1, 1>{1.0 / ka}, 0.001));
}
TEST(ModelsTest, DifferentialDriveFromPhysicalConstants) {
#if __GNUC__ <= 11
auto model = wpi::math::Models::DifferentialDriveFromPhysicalConstants(
wpi::math::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
#else
constexpr auto model = wpi::math::LinearSystemId::DrivetrainVelocitySystem(
wpi::math::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
constexpr auto model =
wpi::math::Models::DifferentialDriveFromPhysicalConstants(
wpi::math::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
#endif
ASSERT_TRUE(model.A().isApprox(
@@ -31,8 +63,8 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) {
wpi::math::Matrixd<2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
}
TEST(LinearSystemIDTest, ElevatorSystem) {
auto model = wpi::math::LinearSystemId::ElevatorSystem(
TEST(ModelsTest, ElevatorFromPhysicalConstants) {
auto model = wpi::math::Models::ElevatorFromPhysicalConstants(
wpi::math::DCMotor::NEO(2), 5_kg, 0.05_m, 12)
.Slice(0);
ASSERT_TRUE(model.A().isApprox(
@@ -42,53 +74,16 @@ TEST(LinearSystemIDTest, ElevatorSystem) {
ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<1, 1>{0.0}, 0.001));
}
TEST(LinearSystemIDTest, FlywheelSystem) {
#if __GNUC__ <= 11
auto model = wpi::math::LinearSystemId::FlywheelSystem(
wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#else
constexpr auto model = wpi::math::LinearSystemId::FlywheelSystem(
wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#endif
ASSERT_TRUE(model.A().isApprox(wpi::math::Matrixd<1, 1>{-26.87032}, 0.001));
ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<1, 1>{1354.166667}, 0.001));
ASSERT_TRUE(model.C().isApprox(wpi::math::Matrixd<1, 1>{1.0}, 0.001));
ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<1, 1>{0.0}, 0.001));
}
TEST(LinearSystemIDTest, DCMotorSystem) {
#if __GNUC__ <= 11
auto model = wpi::math::LinearSystemId::DCMotorSystem(
wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#else
constexpr auto model = wpi::math::LinearSystemId::DCMotorSystem(
wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0);
#endif
ASSERT_TRUE(model.A().isApprox(
wpi::math::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001));
ASSERT_TRUE(
model.B().isApprox(wpi::math::Matrixd<2, 1>{0, 1354.166667}, 0.001));
ASSERT_TRUE(model.C().isApprox(
wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<2, 1>{0.0, 0.0}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyPositionSystem) {
// By controls engineering in frc,
// x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
TEST(ModelsTest, ElevatorFromSysId) {
constexpr double kv = 1.0;
constexpr double ka = 0.5;
#if __GNUC__ <= 11
auto model =
wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
auto model = wpi::math::Models::ElevatorFromSysId(kv * 1_V / 1_mps,
ka * 1_V / 1_mps_sq);
#else
constexpr auto model =
wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
constexpr auto model = wpi::math::Models::ElevatorFromSysId(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
#endif
ASSERT_TRUE(model.A().isApprox(
@@ -97,23 +92,20 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) {
model.B().isApprox(wpi::math::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001));
}
TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
// By controls engineering in frc,
// V = kv * velocity + ka * acceleration
// x-dot = -kv/ka * v + 1/ka \cdot V
TEST(ModelsTest, SingleJointedArmFromSysId) {
constexpr double kv = 1.0;
constexpr double ka = 0.5;
#if __GNUC__ <= 11
auto model =
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
auto model = wpi::math::Models::SingleJointedArmFromSysId(
kv * 1_V / 1_rad_per_s, ka * 1_V / 1_rad_per_s_sq);
#else
constexpr auto model =
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(
kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
constexpr auto model = wpi::math::Models::SingleJointedArmFromSysId(
kv * 1_V / 1_rad_per_s, ka * 1_V / 1_rad_per_s_sq);
#endif
ASSERT_TRUE(model.A().isApprox(wpi::math::Matrixd<1, 1>{-kv / ka}, 0.001));
ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<1, 1>{1.0 / ka}, 0.001));
ASSERT_TRUE(model.A().isApprox(
wpi::math::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
ASSERT_TRUE(
model.B().isApprox(wpi::math::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001));
}

2
wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp → wpimath/src/test/native/cpp/system/proto/DCMotorProtoTest.cpp
View File

@@ -4,7 +4,7 @@
#include <gtest/gtest.h>
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/system/DCMotor.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace wpi::math;

2
wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp → wpimath/src/test/native/cpp/system/struct/DCMotorStructTest.cpp
View File

@@ -4,7 +4,7 @@
#include <gtest/gtest.h>
#include "wpi/math/system/plant/DCMotor.hpp"
#include "wpi/math/system/DCMotor.hpp"
using namespace wpi::math;