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

[wpimath] Fix potential divide-by-zero in RKDP (#5242)

Browse Source 
If f(x, u) has no dynamics, the truncation error can be zero.
This commit is contained in:
Tyler Veness
2023-03-26 17:00:09 -07:00
committed by GitHub
parent 9227b2166e
commit 63512bbbb8
6 changed files with 36 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
View File

@@ -66,7 +66,7 @@ TEST(ElevatorSimTest, MinMax) {
TEST(ElevatorSimTest, Stability) {
frc::sim::ElevatorSim sim{
frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, true};
frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, false};
sim.SetState(frc::Vectord<2>{0.0, 0.0});
sim.SetInput(frc::Vectord<1>{12.0});

2
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
View File

@@ -91,7 +91,7 @@ class ElevatorSimTest {
@Test
void testStability() {
var sim =
new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, true);
new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false);
sim.setState(VecBuilder.fill(0, 0));
sim.setInput(12);

6
wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
View File

@@ -238,7 +238,11 @@ public final class NumericalIntegration {
.times(h))
.normF();
h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
if (truncationError == 0.0) {
h = dtSeconds - dtElapsed;
} else {
h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
}
} while (truncationError > maxError);
dtElapsed += h;

6
wpimath/src/main/native/include/frc/system/NumericalIntegration.h
View File

@@ -122,7 +122,11 @@ T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
(b1[6] - b2[6]) * k7))
.norm();
h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
if (truncationError == 0.0) {
h = dt.value() - dtElapsed;
} else {
h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
}
} while (truncationError > maxError);
dtElapsed += h;

14
wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
View File

@@ -30,6 +30,20 @@ class NumericalIntegrationTest {
assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
}
@Test
void testZeroRKDP() {
var y1 =
NumericalIntegration.rkdp(
(x, u) -> {
return VecBuilder.fill(0);
},
VecBuilder.fill(0),
VecBuilder.fill(0),
0.1);
assertEquals(0.0, y1.get(0, 0), 1e-3);
}
@Test
void testExponentialRKDP() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);

10
wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
View File

@@ -30,6 +30,16 @@ TEST(NumericalIntegrationTest, ExponentialWithU) {
EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
}
// Tests that integrating dx/dt = 0 works with RKDP
TEST(NumericalIntegrationTest, ZeroRKDP) {
frc::Vectord<1> y1 = frc::RKDP(
[](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
return frc::Vectord<1>::Zero();
},
frc::Vectord<1>{0.0}, frc::Vectord<1>{0.0}, 0.1_s);
EXPECT_NEAR(y1(0), 0.0, 1e-3);
}
// Tests that integrating dx/dt = e^x works with RKDP
TEST(NumericalIntegrationTest, ExponentialRKDP) {
frc::Vectord<1> y0{0.0};