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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-04-25 23:45:43 -07:00
parent 2f0990e9d2 0695a4db89
commit 36811211be
79 changed files with 2093 additions and 415 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
View File

@@ -8,6 +8,8 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.wpilibj.UtilityClassTest;
import org.junit.jupiter.api.Test;
@@ -167,4 +169,55 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
assertFalse(MathUtil.isNear(400, -315, 5, 0, 360));
assertFalse(MathUtil.isNear(400, 395, 5, 0, 360));
}
@Test
void testSlewRateTranslation2dUnchanged() {
Translation2d translation1 = new Translation2d(0, 0);
Translation2d translation2 = new Translation2d(2, 2);
Translation2d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50);
Translation2d expected1 = new Translation2d(2, 2);
assertEquals(expected1, result1);
}
@Test
void testSlewRateTranslation2dChanged() {
Translation2d translation3 = new Translation2d(1, 1);
Translation2d translation4 = new Translation2d(3, 3);
Translation2d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2);
Translation2d expected2 =
new Translation2d(1.0 + 1.0 / Math.sqrt(8.0), 1.0 + 1.0 / Math.sqrt(8.0));
assertEquals(expected2, result2);
}
@Test
void testSlewRateTranslation3dUnchanged() {
Translation3d translation1 = new Translation3d(0, 0, 0);
Translation3d translation2 = new Translation3d(2, 2, 2);
Translation3d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50);
Translation3d expected1 = new Translation3d(2, 2, 2);
assertEquals(expected1, result1);
}
@Test
void testSlewRateTranslation3dChanged() {
Translation3d translation3 = new Translation3d(1, 1, 1);
Translation3d translation4 = new Translation3d(3, 3, 3);
Translation3d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2);
Translation3d expected2 =
new Translation3d(
1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0));
assertEquals(expected2, result2);
}
}

118
wpimath/src/test/java/edu/wpi/first/math/controller/ArmFeedforwardTest.java
View File

@@ -18,15 +18,13 @@ import java.util.function.BiFunction;
import org.junit.jupiter.api.Test;
class ArmFeedforwardTest {
private static final double ks = 0.5;
private static final double kg = 1;
private static final double kv = 1.5;
private static final double ka = 2;
private final ArmFeedforward m_armFF = new ArmFeedforward(ks, kg, kv, ka);
/**
* Simulates a single-jointed arm and returns the final state.
*
* @param ks The static gain, in volts.
* @param kv The velocity gain, in volt seconds per radian.
* @param ka The acceleration gain, in volt seconds² per radian.
* @param kg The gravity gain, in volts.
* @param currentAngle The starting angle in radians.
* @param currentVelocity The starting angular velocity in radians per second.
* @param input The input voltage.
@@ -34,7 +32,14 @@ class ArmFeedforwardTest {
* @return The final state as a 2-vector of angle and angular velocity.
*/
private Matrix<N2, N1> simulate(
double currentAngle, double currentVelocity, double input, double dt) {
double ks,
double kv,
double ka,
double kg,
double currentAngle,
double currentVelocity,
double input,
double dt) {
final Matrix<N2, N2> A =
new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.0, 1.0, 0.0, -kv / ka});
final Matrix<N2, N1> B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0.0, 1.0 / ka});
@@ -61,46 +66,115 @@ class ArmFeedforwardTest {
* Calculates a feedforward voltage using overload taking angle, two angular velocities, and dt;
* then simulates an arm using that voltage to verify correctness.
*
* @param armFF The feedforward object.
* @param ks The static gain, in volts.
* @param kv The velocity gain, in volt seconds per radian.
* @param ka The acceleration gain, in volt seconds² per radian.
* @param kg The gravity gain, in volts.
* @param currentAngle The starting angle in radians.
* @param currentVelocity The starting angular velocity in radians per second.
* @param input The input voltage.
* @param dt The simulation time in seconds.
*/
private void calculateAndSimulate(
double currentAngle, double currentVelocity, double nextVelocity, double dt) {
final double input = m_armFF.calculate(currentAngle, currentVelocity, nextVelocity);
assertEquals(nextVelocity, simulate(currentAngle, currentVelocity, input, dt).get(1, 0), 1e-12);
ArmFeedforward armFF,
double ks,
double kv,
double ka,
double kg,
double currentAngle,
double currentVelocity,
double nextVelocity,
double dt) {
final double input = armFF.calculate(currentAngle, currentVelocity, nextVelocity);
assertEquals(
nextVelocity,
simulate(ks, kv, ka, kg, currentAngle, currentVelocity, input, dt).get(1, 0),
1e-4);
}
@Test
void testCalculate() {
final double ks = 0.5;
final double kv = 1.5;
final double ka = 2;
final double kg = 1;
final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
// calculate(angle, angular velocity)
assertEquals(0.5, m_armFF.calculate(Math.PI / 3, 0.0), 0.002);
assertEquals(2.5, m_armFF.calculate(Math.PI / 3, 1.0), 0.002);
assertEquals(0.5, armFF.calculate(Math.PI / 3, 0.0), 0.002);
assertEquals(2.5, armFF.calculate(Math.PI / 3, 1.0), 0.002);
// calculate(currentAngle, currentVelocity, nextAngle, dt)
calculateAndSimulate(Math.PI / 3, 1.0, 1.05, 0.020);
calculateAndSimulate(Math.PI / 3, 1.0, 0.95, 0.020);
calculateAndSimulate(-Math.PI / 3, 1.0, 1.05, 0.020);
calculateAndSimulate(-Math.PI / 3, 1.0, 0.95, 0.020);
calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 1.05, 0.020);
calculateAndSimulate(armFF, ks, kv, ka, kg, Math.PI / 3, 1.0, 0.95, 0.020);
calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 1.05, 0.020);
calculateAndSimulate(armFF, ks, kv, ka, kg, -Math.PI / 3, 1.0, 0.95, 0.020);
}
@Test
void testCalculateIllConditionedModel() {
final double ks = 0.39671;
final double kv = 2.7167;
final double ka = 1e-2;
final double kg = 0.2708;
final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
final double currentAngle = 1.0;
final double currentVelocity = 0.02;
final double nextVelocity = 0.0;
var averageAccel = (nextVelocity - currentVelocity) / 0.02;
assertEquals(
armFF.calculate(currentAngle, currentVelocity, nextVelocity),
ks + kv * currentVelocity + ka * averageAccel + kg * Math.cos(currentAngle));
}
@Test
void testCalculateIllConditionedGradient() {
final double ks = 0.39671;
final double kv = 2.7167;
final double ka = 0.50799;
final double kg = 0.2708;
final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
calculateAndSimulate(armFF, ks, kv, ka, kg, 1.0, 0.02, 0.0, 0.02);
}
@Test
void testAchievableVelocity() {
assertEquals(6, m_armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
assertEquals(-9, m_armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
final double ks = 0.5;
final double kv = 1.5;
final double ka = 2;
final double kg = 1;
final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
assertEquals(6, armFF.maxAchievableVelocity(12, Math.PI / 3, 1), 0.002);
assertEquals(-9, armFF.minAchievableVelocity(11.5, Math.PI / 3, 1), 0.002);
}
@Test
void testAchievableAcceleration() {
assertEquals(4.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
assertEquals(6.75, m_armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
assertEquals(-7.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
assertEquals(-5.25, m_armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
final double ks = 0.5;
final double kv = 1.5;
final double ka = 2;
final double kg = 1;
final ArmFeedforward armFF = new ArmFeedforward(ks, kg, kv, ka);
assertEquals(4.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
assertEquals(6.75, armFF.maxAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
assertEquals(-7.25, armFF.minAchievableAcceleration(12, Math.PI / 3, 1), 0.002);
assertEquals(-5.25, armFF.minAchievableAcceleration(12, Math.PI / 3, -1), 0.002);
}
@Test
void testNegativeGains() {
final double ks = 0.5;
final double kv = 1.5;
final double ka = 2;
final double kg = 1;
assertAll(
() ->
assertThrows(IllegalArgumentException.class, () -> new ArmFeedforward(ks, kg, -kv, ka)),

173
wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
View File

@@ -9,6 +9,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.StateSpaceUtil;
@@ -18,6 +19,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.math.system.Discretization;
import edu.wpi.first.math.system.NumericalIntegration;
@@ -26,11 +28,13 @@ import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import org.junit.jupiter.api.Test;
class UnscentedKalmanFilterTest {
private static Matrix<N5, N1> getDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
private static Matrix<N5, N1> driveDynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
var motors = DCMotor.getCIM(2);
// var gLow = 15.32; // Low gear ratio
@@ -60,17 +64,17 @@ class UnscentedKalmanFilterTest {
}
@SuppressWarnings("PMD.UnusedFormalParameter")
private static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
private static Matrix<N3, N1> driveLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
}
@SuppressWarnings("PMD.UnusedFormalParameter")
private static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
private static Matrix<N5, N1> driveGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return x.copy();
}
@Test
void testInit() {
void testDriveInit() {
var dt = 0.005;
assertDoesNotThrow(
() -> {
@@ -78,8 +82,8 @@ class UnscentedKalmanFilterTest {
new UnscentedKalmanFilter<>(
Nat.N5(),
Nat.N3(),
UnscentedKalmanFilterTest::getDynamics,
UnscentedKalmanFilterTest::getLocalMeasurementModel,
UnscentedKalmanFilterTest::driveDynamics,
UnscentedKalmanFilterTest::driveLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.01, 0.01),
AngleStatistics.angleMean(2),
@@ -92,10 +96,10 @@ class UnscentedKalmanFilterTest {
var u = VecBuilder.fill(12.0, 12.0);
observer.predict(u, dt);
var localY = getLocalMeasurementModel(observer.getXhat(), u);
var localY = driveLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
var globalY = driveGlobalMeasurementModel(observer.getXhat(), u);
var R =
StateSpaceUtil.makeCovarianceMatrix(
Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.01, 0.01));
@@ -103,7 +107,7 @@ class UnscentedKalmanFilterTest {
Nat.N5(),
u,
globalY,
UnscentedKalmanFilterTest::getGlobalMeasurementModel,
UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
R,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
@@ -113,16 +117,16 @@ class UnscentedKalmanFilterTest {
}
@Test
void testConvergence() {
double dt = 0.005;
double rb = 0.8382 / 2.0; // Robot radius
void testDriveConvergence() {
final double dt = 0.005;
final double rb = 0.8382 / 2.0; // Robot radius
UnscentedKalmanFilter<N5, N2, N3> observer =
new UnscentedKalmanFilter<>(
Nat.N5(),
Nat.N3(),
UnscentedKalmanFilterTest::getDynamics,
UnscentedKalmanFilterTest::getLocalMeasurementModel,
UnscentedKalmanFilterTest::driveDynamics,
UnscentedKalmanFilterTest::driveLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.5, 0.5),
AngleStatistics.angleMean(2),
@@ -146,7 +150,7 @@ class UnscentedKalmanFilterTest {
NumericalJacobian.numericalJacobianU(
Nat.N5(),
Nat.N2(),
UnscentedKalmanFilterTest::getDynamics,
UnscentedKalmanFilterTest::driveDynamics,
new Matrix<>(Nat.N5(), Nat.N1()),
new Matrix<>(Nat.N2(), Nat.N1()));
@@ -161,7 +165,7 @@ class UnscentedKalmanFilterTest {
var trueXhat = observer.getXhat();
double totalTime = trajectory.getTotalTime();
for (int i = 0; i < (totalTime / dt); i++) {
for (int i = 0; i < (totalTime / dt); ++i) {
var ref = trajectory.sample(dt * i);
double vl = ref.velocity * (1 - (ref.curvature * rb));
double vr = ref.velocity * (1 + (ref.curvature * rb));
@@ -174,24 +178,26 @@ class UnscentedKalmanFilterTest {
vl,
vr);
Matrix<N3, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
Matrix<N3, N1> localY =
driveLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
var noiseStdDev = VecBuilder.fill(0.0001, 0.5, 0.5);
observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
var rdot = nextR.minus(r).div(dt);
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
u = new Matrix<>(B.solve(rdot.minus(driveDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
observer.predict(u, dt);
r = nextR;
trueXhat = NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dt);
trueXhat =
NumericalIntegration.rk4(UnscentedKalmanFilterTest::driveDynamics, trueXhat, u, dt);
}
var localY = getLocalMeasurementModel(trueXhat, u);
var localY = driveLocalMeasurementModel(trueXhat, u);
observer.correct(u, localY);
var globalY = getGlobalMeasurementModel(trueXhat, u);
var globalY = driveGlobalMeasurementModel(trueXhat, u);
var R =
StateSpaceUtil.makeCovarianceMatrix(
Nat.N5(), VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
@@ -199,7 +205,7 @@ class UnscentedKalmanFilterTest {
Nat.N5(),
u,
globalY,
UnscentedKalmanFilterTest::getGlobalMeasurementModel,
UnscentedKalmanFilterTest::driveGlobalMeasurementModel,
R,
AngleStatistics.angleMean(2),
AngleStatistics.angleResidual(2),
@@ -236,7 +242,7 @@ class UnscentedKalmanFilterTest {
Matrix<N1, N1> ref = VecBuilder.fill(100);
Matrix<N1, N1> u = VecBuilder.fill(0);
for (int i = 0; i < (2.0 / dt); i++) {
for (int i = 0; i < (2.0 / dt); ++i) {
observer.predict(u, dt);
u = discB.solve(ref.minus(discA.times(ref)));
@@ -264,4 +270,125 @@ class UnscentedKalmanFilterTest {
assertTrue(observer.getP().isEqual(P, 1e-9));
}
// Second system, single motor feedforward estimator
private static Matrix<N4, N1> motorDynamics(Matrix<N4, N1> x, Matrix<N1, N1> u) {
double v = x.get(1, 0);
double kV = x.get(2, 0);
double kA = x.get(3, 0);
double V = u.get(0, 0);
double a = -kV / kA * v + 1.0 / kA * V;
return MatBuilder.fill(Nat.N4(), Nat.N1(), v, a, 0, 0);
}
private static Matrix<N3, N1> motorMeasurementModel(Matrix<N4, N1> x, Matrix<N1, N1> u) {
double p = x.get(0, 0);
double v = x.get(1, 0);
double kV = x.get(2, 0);
double kA = x.get(3, 0);
double V = u.get(0, 0);
double a = -kV / kA * v + 1.0 / kA * V;
return MatBuilder.fill(Nat.N3(), Nat.N1(), p, v, a);
}
private static Matrix<N1, N1> motorControlInput(double t) {
return MatBuilder.fill(
Nat.N1(),
Nat.N1(),
MathUtil.clamp(
8 * Math.sin(Math.PI * Math.sqrt(2.0) * t)
+ 6 * Math.sin(Math.PI * Math.sqrt(3.0) * t)
+ 4 * Math.sin(Math.PI * Math.sqrt(5.0) * t),
-12.0,
12.0));
}
@Test
void testMotorConvergence() {
final double dt = 0.01;
final int steps = 500;
final double true_kV = 3;
final double true_kA = 0.2;
final double pos_stddev = 0.02;
final double vel_stddev = 0.1;
final double accel_stddev = 0.1;
var states =
new ArrayList<>(
Collections.nCopies(
steps + 1, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 0.0, 0.0)));
var inputs =
new ArrayList<>(Collections.nCopies(steps, MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0)));
var measurements =
new ArrayList<>(
Collections.nCopies(steps + 1, MatBuilder.fill(Nat.N3(), Nat.N1(), 0.0, 0.0, 0.0)));
states.set(0, MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, true_kV, true_kA));
var A =
MatBuilder.fill(
Nat.N4(),
Nat.N4(),
0.0,
1.0,
0.0,
0.0,
0.0,
-true_kV / true_kA,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0);
var B = MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 1.0 / true_kA, 0.0, 0.0);
var discABPair = Discretization.discretizeAB(A, B, dt);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
for (int i = 0; i < steps; ++i) {
inputs.set(i, motorControlInput(i * dt));
states.set(i + 1, discA.times(states.get(i)).plus(discB.times(inputs.get(i))));
measurements.set(
i,
motorMeasurementModel(states.get(i + 1), inputs.get(i))
.plus(
StateSpaceUtil.makeWhiteNoiseVector(
VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev))));
}
var P0 =
MatBuilder.fill(
Nat.N4(), Nat.N4(), 0.001, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 10, 0.0, 0.0,
0.0, 0.0, 10);
var observer =
new UnscentedKalmanFilter<N4, N1, N3>(
Nat.N4(),
Nat.N3(),
UnscentedKalmanFilterTest::motorDynamics,
UnscentedKalmanFilterTest::motorMeasurementModel,
VecBuilder.fill(0.1, 1.0, 1e-10, 1e-10),
VecBuilder.fill(pos_stddev, vel_stddev, accel_stddev),
dt);
observer.setXhat(MatBuilder.fill(Nat.N4(), Nat.N1(), 0.0, 0.0, 2.0, 2.0));
observer.setP(P0);
for (int i = 0; i < steps; ++i) {
observer.predict(inputs.get(i), dt);
observer.correct(inputs.get(i), measurements.get(i));
}
assertEquals(true_kV, observer.getXhat(2), true_kV * 0.5);
assertEquals(true_kA, observer.getXhat(3), true_kA * 0.5);
}
}

19
wpimath/src/test/java/edu/wpi/first/math/filter/DebouncerTest.java
View File

@@ -4,7 +4,9 @@
package edu.wpi.first.math.filter;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertSame;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.util.WPIUtilJNI;
@@ -67,4 +69,21 @@ class DebouncerTest {
assertFalse(debouncer.calculate(false));
}
@Test
void debounceParamsTest() {
var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kBoth);
assertEquals(debouncer.getDebounceTime(), 0.02);
assertSame(debouncer.getDebounceType(), Debouncer.DebounceType.kBoth);
debouncer.setDebounceTime(0.1);
assertEquals(debouncer.getDebounceTime(), 0.1);
debouncer.setDebounceType(Debouncer.DebounceType.kFalling);
assertSame(debouncer.getDebounceType(), Debouncer.DebounceType.kFalling);
assertTrue(debouncer.calculate(false));
}
}

7
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
View File

@@ -246,4 +246,11 @@ class TrapezoidProfileTest {
}
}
}
@Test
void initalizationOfCurrentState() {
var profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(1, 1));
assertNear(profile.timeLeftUntil(0), 0, 1e-10);
assertNear(profile.totalTime(), 0, 1e-10);
}
}

59
wpimath/src/test/native/cpp/MathUtilTest.cpp
View File

@@ -3,11 +3,17 @@
// the WPILib BSD license file in the root directory of this project.
#include <limits>
#include <numbers>
#include <gtest/gtest.h>
#include "frc/MathUtil.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Translation3d.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/time.h"
#include "units/velocity.h"
#define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())
@@ -164,3 +170,56 @@ TEST(MathUtilTest, IsNear) {
EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
}
TEST(MathUtilTest, Translation2dSlewRateLimitUnchanged) {
const frc::Translation2d translation1{0_m, 0_m};
const frc::Translation2d translation2{2_m, 2_m};
const frc::Translation2d result1 =
frc::SlewRateLimit(translation1, translation2, 1_s, 50_mps);
const frc::Translation2d expected1{2_m, 2_m};
EXPECT_EQ(result1, expected1);
}
TEST(MathUtilTest, Translation2dSlewRateLimitChanged) {
const frc::Translation2d translation3{1_m, 1_m};
const frc::Translation2d translation4{3_m, 3_m};
const frc::Translation2d result2 =
frc::SlewRateLimit(translation3, translation4, 0.25_s, 2_mps);
const frc::Translation2d expected2{
units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)},
units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)}};
EXPECT_EQ(result2, expected2);
}
TEST(MathUtilTest, Translation3dSlewRateLimitUnchanged) {
const frc::Translation3d translation1{0_m, 0_m, 0_m};
const frc::Translation3d translation2{2_m, 2_m, 2_m};
const frc::Translation3d result1 =
frc::SlewRateLimit(translation1, translation2, 1_s, 50.0_mps);
const frc::Translation3d expected1{2_m, 2_m, 2_m};
EXPECT_EQ(result1, expected1);
}
TEST(MathUtilTest, Translation3dSlewRateLimitChanged) {
const frc::Translation3d translation3{1_m, 1_m, 1_m};
const frc::Translation3d translation4{3_m, 3_m, 3_m};
const frc::Translation3d result2 =
frc::SlewRateLimit(translation3, translation4, 0.25_s, 2.0_mps);
const frc::Translation3d expected2{
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3}};
EXPECT_EQ(result2, expected2);
}

100
wpimath/src/test/native/cpp/controller/ArmFeedforwardTest.cpp
View File

@@ -15,27 +15,32 @@
#include "units/time.h"
#include "units/voltage.h"
static constexpr auto Ks = 0.5_V;
static constexpr auto Kv = 1.5_V / 1_rad_per_s;
static constexpr auto Ka = 2_V / 1_rad_per_s_sq;
static constexpr auto Kg = 1_V;
namespace {
using Ks_unit = decltype(1_V);
using Kv_unit = decltype(1_V / 1_rad_per_s);
using Ka_unit = decltype(1_V / 1_rad_per_s_sq);
using Kg_unit = decltype(1_V);
/**
* Simulates a single-jointed arm and returns the final state.
*
* @param Ks The static gain, in volts.
* @param Kv The velocity gain, in volt seconds per radian.
* @param Ka The acceleration gain, in volt seconds² per radian.
* @param Kg The gravity gain, in volts.
* @param currentAngle The starting angle.
* @param currentVelocity The starting angular velocity.
* @param input The input voltage.
* @param dt The simulation time.
* @return The final state as a 2-vector of angle and angular velocity.
*/
frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle,
frc::Matrixd<2, 1> Simulate(Ks_unit Ks, Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::volt_t input, units::second_t dt) {
constexpr frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
constexpr frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
frc::Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -Kv.value() / Ka.value()}};
frc::Matrixd<2, 1> B{{0.0}, {1.0 / Ka.value()}};
return frc::RK4(
[&](const frc::Matrixd<2, 1>& x,
@@ -52,24 +57,35 @@ frc::Matrixd<2, 1> Simulate(units::radian_t currentAngle,
* Simulates a single-jointed arm and returns the final state.
*
* @param armFF The feedforward object.
* @param Ks The static gain, in volts.
* @param Kv The velocity gain, in volt seconds per radian.
* @param Ka The acceleration gain, in volt seconds² per radian.
* @param Kg The gravity gain, in volts.
* @param currentAngle The starting angle.
* @param currentVelocity The starting angular velocity.
* @param input The input voltage.
* @param dt The simulation time.
*/
void CalculateAndSimulate(const frc::ArmFeedforward& armFF,
void CalculateAndSimulate(const frc::ArmFeedforward& armFF, Ks_unit Ks,
Kv_unit Kv, Ka_unit Ka, Kg_unit Kg,
units::radian_t currentAngle,
units::radians_per_second_t currentVelocity,
units::radians_per_second_t nextVelocity,
units::second_t dt) {
auto input = armFF.Calculate(currentAngle, currentVelocity, nextVelocity);
EXPECT_NEAR(nextVelocity.value(),
Simulate(currentAngle, currentVelocity, input, dt)(1), 1e-12);
EXPECT_NEAR(
nextVelocity.value(),
Simulate(Ks, Kv, Ka, Kg, currentAngle, currentVelocity, input, dt)(1),
1e-4);
}
} // namespace
TEST(ArmFeedforwardTest, Calculate) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
// Calculate(angle, angular velocity)
@@ -81,18 +97,54 @@ TEST(ArmFeedforwardTest, Calculate) {
0.002);
// Calculate(currentAngle, currentVelocity, nextAngle, dt)
CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
0.95_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, -std::numbers::pi / 3 * 1_rad, 1_rad_per_s,
0.95_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 0.95_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 1.05_rad_per_s, 20_ms);
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, -std::numbers::pi / 3 * 1_rad,
1_rad_per_s, 0.95_rad_per_s, 20_ms);
}
TEST(ArmFeedforwardTest, CalculateIllConditionedModel) {
constexpr auto Ks = 0.39671_V;
constexpr auto Kv = 2.7167_V / 1_rad_per_s;
constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
constexpr auto Kg = 0.2708_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
constexpr auto currentAngle = 1_rad;
constexpr auto currentVelocity = 0.02_rad_per_s;
constexpr auto nextVelocity = 0_rad_per_s;
constexpr auto averageAccel = (nextVelocity - currentVelocity) / 20_ms;
EXPECT_DOUBLE_EQ(
armFF.Calculate(currentAngle, currentVelocity, nextVelocity).value(),
(Ks + Kv * currentVelocity + Ka * averageAccel +
Kg * units::math::cos(currentAngle))
.value());
}
TEST(ArmFeedforwardTest, CalculateIllConditionedGradient) {
constexpr auto Ks = 0.39671_V;
constexpr auto Kv = 2.7167_V / 1_rad_per_s;
constexpr auto Ka = 0.50799_V / 1_rad_per_s_sq;
constexpr auto Kg = 0.2708_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
CalculateAndSimulate(armFF, Ks, Kv, Ka, Kg, 1_rad, 0.02_rad_per_s,
0_rad_per_s, 20_ms);
}
TEST(ArmFeedforwardTest, AchievableVelocity) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableVelocity(12_V, std::numbers::pi / 3 * 1_rad,
1_rad_per_s_sq)
@@ -106,7 +158,12 @@ TEST(ArmFeedforwardTest, AchievableVelocity) {
}
TEST(ArmFeedforwardTest, AchievableAcceleration) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};
EXPECT_NEAR(armFF
.MaxAchievableAcceleration(12_V, std::numbers::pi / 3 * 1_rad,
1_rad_per_s)
@@ -130,7 +187,12 @@ TEST(ArmFeedforwardTest, AchievableAcceleration) {
}
TEST(ArmFeedforwardTest, NegativeGains) {
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 1.5_V / 1_rad_per_s;
constexpr auto Ka = 2_V / 1_rad_per_s_sq;
constexpr auto Kg = 1_V;
frc::ArmFeedforward armFF{Ks, Kg, -Kv, -Ka};
EXPECT_EQ(armFF.GetKv().value(), 0);
EXPECT_EQ(armFF.GetKa().value(), 0);
}

166
wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
View File

@@ -2,7 +2,9 @@
// 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 <algorithm>
#include <cmath>
#include <numbers>
#include <vector>
#include <Eigen/QR>
@@ -12,15 +14,19 @@
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/AngleStatistics.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "units/moment_of_inertia.h"
namespace {
frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
// First test system, differential drive
frc::Vectord<5> DriveDynamics(const frc::Vectord<5>& x,
const frc::Vectord<2>& u) {
auto motors = frc::DCMotor::CIM(2);
// constexpr double Glow = 15.32; // Low gear ratio
@@ -51,22 +57,21 @@ frc::Vectord<5> Dynamics(const frc::Vectord<5>& x, const frc::Vectord<2>& u) {
k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
}
frc::Vectord<3> LocalMeasurementModel(
frc::Vectord<3> DriveLocalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<3>{x(2), x(3), x(4)};
}
frc::Vectord<5> GlobalMeasurementModel(
frc::Vectord<5> DriveGlobalMeasurementModel(
const frc::Vectord<5>& x, [[maybe_unused]] const frc::Vectord<2>& u) {
return frc::Vectord<5>{x(0), x(1), x(2), x(3), x(4)};
}
} // namespace
TEST(UnscentedKalmanFilterTest, Init) {
TEST(UnscentedKalmanFilterTest, DriveInit) {
constexpr auto dt = 5_ms;
frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.01, 0.01},
frc::AngleMean<5, 5>(2),
@@ -78,22 +83,22 @@ TEST(UnscentedKalmanFilterTest, Init) {
frc::Vectord<2> u{12.0, 12.0};
observer.Predict(u, dt);
auto localY = LocalMeasurementModel(observer.Xhat(), u);
auto localY = DriveLocalMeasurementModel(observer.Xhat(), u);
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
auto globalY = DriveGlobalMeasurementModel(observer.Xhat(), u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
}
TEST(UnscentedKalmanFilterTest, Convergence) {
TEST(UnscentedKalmanFilterTest, DriveConvergence) {
constexpr auto dt = 5_ms;
constexpr auto rb = 0.8382_m / 2.0; // Robot radius
frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
LocalMeasurementModel,
frc::UnscentedKalmanFilter<5, 2, 3> observer{DriveDynamics,
DriveLocalMeasurementModel,
{0.5, 0.5, 10.0, 1.0, 1.0},
{0.0001, 0.5, 0.5},
frc::AngleMean<5, 5>(2),
@@ -112,8 +117,8 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
frc::Vectord<5> r = frc::Vectord<5>::Zero();
frc::Vectord<2> u = frc::Vectord<2>::Zero();
auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics, frc::Vectord<5>::Zero(),
frc::Vectord<2>::Zero());
auto B = frc::NumericalJacobianU<5, 5, 2>(
DriveDynamics, frc::Vectord<5>::Zero(), frc::Vectord<2>::Zero());
observer.SetXhat(frc::Vectord<5>{
trajectory.InitialPose().Translation().X().value(),
@@ -134,24 +139,25 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
auto localY = LocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
auto localY = DriveLocalMeasurementModel(trueXhat, frc::Vectord<2>::Zero());
observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
frc::Vectord<5> rdot = (nextR - r) / dt.value();
u = B.householderQr().solve(rdot - Dynamics(r, frc::Vectord<2>::Zero()));
u = B.householderQr().solve(rdot -
DriveDynamics(r, frc::Vectord<2>::Zero()));
observer.Predict(u, dt);
r = nextR;
trueXhat = frc::RK4(Dynamics, trueXhat, u, dt);
trueXhat = frc::RK4(DriveDynamics, trueXhat, u, dt);
}
auto localY = LocalMeasurementModel(trueXhat, u);
auto localY = DriveLocalMeasurementModel(trueXhat, u);
observer.Correct(u, localY);
auto globalY = GlobalMeasurementModel(trueXhat, u);
auto globalY = DriveGlobalMeasurementModel(trueXhat, u);
auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
observer.Correct<5>(u, globalY, DriveGlobalMeasurementModel, R,
frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
@@ -168,6 +174,37 @@ TEST(UnscentedKalmanFilterTest, Convergence) {
EXPECT_NEAR(0.0, observer.Xhat(4), 0.1);
}
TEST(UnscentedKalmanFilterTest, LinearUKF) {
constexpr units::second_t dt = 20_ms;
auto plant = frc::LinearSystemId::IdentifyVelocitySystem<units::meters>(
0.02_V / 1_mps, 0.006_V / 1_mps_sq);
frc::UnscentedKalmanFilter<1, 1, 1> observer{
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return plant.A() * x + plant.B() * u;
},
[&](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return plant.CalculateY(x, u);
},
{0.05},
{1.0},
dt};
frc::Matrixd<1, 1> discA;
frc::Matrixd<1, 1> discB;
frc::DiscretizeAB<1, 1>(plant.A(), plant.B(), dt, &discA, &discB);
frc::Vectord<1> ref{100.0};
frc::Vectord<1> u{0.0};
for (int i = 0; i < 2.0 / dt.value(); ++i) {
observer.Predict(u, dt);
u = discB.householderQr().solve(ref - discA * ref);
}
EXPECT_NEAR(ref(0, 0), observer.Xhat(0), 5);
}
TEST(UnscentedKalmanFilterTest, RoundTripP) {
constexpr auto dt = 5_ms;
@@ -183,3 +220,90 @@ TEST(UnscentedKalmanFilterTest, RoundTripP) {
ASSERT_TRUE(observer.P().isApprox(P));
}
// Second system, single motor feedforward estimator
frc::Vectord<4> MotorDynamics(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
double v = x(1);
double kV = x(2);
double kA = x(3);
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<4>{v, a, 0.0, 0.0};
}
frc::Vectord<3> MotorMeasurementModel(const frc::Vectord<4>& x,
const frc::Vectord<1>& u) {
double p = x(0);
double v = x(1);
double kV = x(2);
double kA = x(3);
double V = u(0);
double a = -kV / kA * v + 1.0 / kA * V;
return frc::Vectord<3>{p, v, a};
}
frc::Vectord<1> MotorControlInput(double t) {
return frc::Vectord<1>{
std::clamp(8 * std::sin(std::numbers::pi * std::sqrt(2.0) * t) +
6 * std::sin(std::numbers::pi * std::sqrt(3.0) * t) +
4 * std::sin(std::numbers::pi * std::sqrt(5.0) * t),
-12.0, 12.0)};
}
TEST(UnscentedKalmanFilterTest, MotorConvergence) {
constexpr units::second_t dt = 10_ms;
constexpr int steps = 500;
constexpr double true_kV = 3;
constexpr double true_kA = 0.2;
constexpr double pos_stddev = 0.02;
constexpr double vel_stddev = 0.1;
constexpr double accel_stddev = 0.1;
std::vector<frc::Vectord<4>> states(steps + 1);
std::vector<frc::Vectord<1>> inputs(steps);
std::vector<frc::Vectord<3>> measurements(steps);
states[0] = frc::Vectord<4>{{0.0}, {0.0}, {true_kV}, {true_kA}};
constexpr frc::Matrixd<4, 4> A{{0.0, 1.0, 0.0, 0.0},
{0.0, -true_kV / true_kA, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0},
{0.0, 0.0, 0.0, 0.0}};
constexpr frc::Matrixd<4, 1> B{{0.0}, {1.0 / true_kA}, {0.0}, {0.0}};
frc::Matrixd<4, 4> discA;
frc::Matrixd<4, 1> discB;
frc::DiscretizeAB(A, B, dt, &discA, &discB);
for (int i = 0; i < steps; ++i) {
inputs[i] = MotorControlInput(i * dt.value());
states[i + 1] = discA * states[i] + discB * inputs[i];
measurements[i] =
MotorMeasurementModel(states[i + 1], inputs[i]) +
frc::MakeWhiteNoiseVector(pos_stddev, vel_stddev, accel_stddev);
}
frc::Vectord<4> P0{0.001, 0.001, 10, 10};
frc::UnscentedKalmanFilter<4, 1, 3> observer{
MotorDynamics, MotorMeasurementModel, wpi::array{0.1, 1.0, 1e-10, 1e-10},
wpi::array{pos_stddev, vel_stddev, accel_stddev}, dt};
observer.SetXhat(frc::Vectord<4>{0.0, 0.0, 2.0, 2.0});
observer.SetP(P0.asDiagonal());
for (int i = 0; i < steps; ++i) {
observer.Predict(inputs[i], dt);
observer.Correct(inputs[i], measurements[i]);
}
EXPECT_NEAR(true_kV, observer.Xhat(2), true_kV * 0.5);
EXPECT_NEAR(true_kA, observer.Xhat(3), true_kA * 0.5);
}
} // namespace

19
wpimath/src/test/native/cpp/filter/DebouncerTest.cpp
View File

@@ -56,3 +56,22 @@ TEST_F(DebouncerTest, DebounceBoth) {
EXPECT_FALSE(debouncer.Calculate(false));
}
TEST_F(DebouncerTest, DebounceParams) {
frc::Debouncer debouncer{20_ms, frc::Debouncer::DebounceType::kBoth};
EXPECT_TRUE(debouncer.GetDebounceTime() == 20_ms);
EXPECT_TRUE(debouncer.GetDebounceType() ==
frc::Debouncer::DebounceType::kBoth);
debouncer.SetDebounceTime(100_ms);
EXPECT_TRUE(debouncer.GetDebounceTime() == 100_ms);
debouncer.SetDebounceType(frc::Debouncer::DebounceType::kFalling);
EXPECT_TRUE(debouncer.GetDebounceType() ==
frc::Debouncer::DebounceType::kFalling);
EXPECT_TRUE(debouncer.Calculate(false));
}

7
wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
View File

@@ -234,3 +234,10 @@ TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
}
}
}
TEST(TrapezoidProfileTest, InitalizationOfCurrentState) {
frc::TrapezoidProfile<units::meter>::Constraints constraints{1_mps, 1_mps_sq};
frc::TrapezoidProfile<units::meter> profile{constraints};
EXPECT_NEAR_UNITS(profile.TimeLeftUntil(0_m), 0_s, 1e-10_s);
EXPECT_NEAR_UNITS(profile.TotalTime(), 0_s, 1e-10_s);
}