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

[wpimath] Make SimpleMotorFeedforward only support discrete feedforward (#6647)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-07-16 20:23:11 -04:00
committed by GitHub
parent 5f261a88af
commit 30c7632ab8
31 changed files with 540 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
View File

@@ -4,12 +4,14 @@
package edu.wpi.first.math.controller;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.units.MutableMeasure;
import org.junit.jupiter.api.Test;
class SimpleMotorFeedforwardTest {
@@ -24,22 +26,27 @@ class SimpleMotorFeedforwardTest {
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / Ka);
var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka);
var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka, dt);
var r = VecBuilder.fill(2.0);
var nextR = VecBuilder.fill(3.0);
var currentVelocity = MutableMeasure.ofBaseUnits(2.0, RadiansPerSecond);
var nextVelocity = MutableMeasure.ofBaseUnits(3.0, RadiansPerSecond);
assertEquals(37.52499583432516 + 0.5, simpleMotor.calculate(2.0, 3.0, dt), 0.002);
assertEquals(
37.52499583432516 + 0.5,
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
0.002);
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + Ks,
simpleMotor.calculate(2.0, 3.0, dt),
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
0.002);
// These won't match exactly. It's just an approximation to make sure they're
// in the same ballpark.
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + Ks,
simpleMotor.calculate(2.0, 1.0 / dt),
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
2.0);
}
}

36
wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
View File

@@ -4,6 +4,8 @@
package edu.wpi.first.math.trajectory;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -41,33 +43,49 @@ class DifferentialDriveVoltageConstraintTest {
point.velocityMetersPerSecond,
0,
point.velocityMetersPerSecond * point.curvatureRadPerMeter);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
t += dt;
var acceleration = point.accelerationMetersPerSecondSq;
// Not really a strictly-correct test as we're using the chassis accel instead of the
// wheel accel, but much easier than doing it "properly" and a reasonable check anyway
assertAll(
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.leftMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.leftMetersPerSecond + dt * acceleration))
.in(Volts)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.leftMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.leftMetersPerSecond + dt * acceleration))
.in(Volts)
>= -maxVoltage - 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.rightMetersPerSecond + dt * acceleration))
.in(Volts)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.rightMetersPerSecond + dt * acceleration))
.in(Volts)
>= -maxVoltage - 0.05));
}
}

11
wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java
View File

@@ -4,19 +4,21 @@
package edu.wpi.first.math.trajectory;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.units.MutableMeasure;
import java.util.List;
import org.junit.jupiter.api.Test;
class ExponentialProfileTest {
private static final double kDt = 0.01;
private static final SimpleMotorFeedforward feedforward =
new SimpleMotorFeedforward(0, 2.5629, 0.43277);
new SimpleMotorFeedforward(0, 2.5629, 0.43277, kDt);
private static final ExponentialProfile.Constraints constraints =
ExponentialProfile.Constraints.fromCharacteristics(12, 2.5629, 0.43277);
@@ -43,10 +45,11 @@ class ExponentialProfileTest {
private static ExponentialProfile.State checkDynamics(
ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) {
var next = profile.calculate(kDt, current, goal);
var currentVelocity = MutableMeasure.ofBaseUnits(current.velocity, RadiansPerSecond);
var nextVelocity = MutableMeasure.ofBaseUnits(next.velocity, RadiansPerSecond);
var signal = feedforward.calculate(currentVelocity, nextVelocity);
var signal = feedforward.calculate(current.velocity, next.velocity, kDt);
assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9);
assertTrue(Math.abs(signal.magnitude()) < constraints.maxInput + 1e-9);
return next;
}

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

@@ -12,35 +12,34 @@
#include "units/acceleration.h"
#include "units/length.h"
#include "units/time.h"
#include "units/velocity.h"
namespace frc {
TEST(SimpleMotorFeedforwardTest, Calculate) {
double Ks = 0.5;
double Kv = 3.0;
double Ka = 0.6;
auto dt = 0.02_s;
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 3_V / 1_mps;
constexpr auto Ka = 0.6_V / 1_mps_sq;
constexpr units::second_t dt = 20_ms;
Matrixd<1, 1> A{-Kv / Ka};
Matrixd<1, 1> B{1.0 / Ka};
constexpr Matrixd<1, 1> A{{-Kv.value() / Ka.value()}};
constexpr Matrixd<1, 1> B{{1.0 / Ka.value()}};
frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
frc::SimpleMotorFeedforward<units::meter> simpleMotor{
units::volt_t{Ks}, units::volt_t{Kv} / 1_mps,
units::volt_t{Ka} / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> simpleMotor{Ks, Kv, Ka};
Vectord<1> r{2.0};
Vectord<1> nextR{3.0};
constexpr Vectord<1> r{{2.0}};
constexpr Vectord<1> nextR{{3.0}};
EXPECT_NEAR(37.524995834325161 + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
EXPECT_NEAR((37.524995834325161_V + Ks).value(),
simpleMotor.Calculate(2_mps, 3_mps).value(), 0.002);
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value(),
simpleMotor.Calculate(2_mps, 3_mps).value(), 0.002);
// These won't match exactly. It's just an approximation to make sure they're
// in the same ballpark.
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
simpleMotor.Calculate(2_mps, 1_mps / dt).value(), 2.0);
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value(),
simpleMotor.Calculate(2_mps, 3_mps).value(), 2.0);
}
} // namespace frc

12
wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
View File

@@ -45,17 +45,19 @@ TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
point.velocity * point.curvature};
auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
auto acceleration = point.acceleration;
// Not really a strictly-correct test as we're using the chassis accel
// instead of the wheel accel, but much easier than doing it "properly" and
// a reasonable check anyway
EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) <
EXPECT_TRUE(feedforward.Calculate(left, left + acceleration * dt) <
maxVoltage + 0.05_V);
EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) >
EXPECT_TRUE(feedforward.Calculate(left, left + acceleration * dt) >
-maxVoltage - 0.05_V);
EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) <
EXPECT_TRUE(feedforward.Calculate(right,
right + acceleration * dt) <
maxVoltage + 0.05_V);
EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) >
EXPECT_TRUE(feedforward.Calculate(right, right + acceleration * dt) >
-maxVoltage - 0.05_V);
}
}

52
wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp
View File

@@ -41,9 +41,9 @@ frc::ExponentialProfile<units::meter, units::volts>::State CheckDynamics(
frc::ExponentialProfile<units::meter, units::volts>::State current,
frc::ExponentialProfile<units::meter, units::volts>::State goal) {
auto next = profile.Calculate(kDt, current, goal);
auto signal = feedforward.Calculate(current.velocity, next.velocity, kDt);
auto signal = feedforward.Calculate(current.velocity, next.velocity);
EXPECT_LE(units::math::abs(signal), constraints.maxInput + 1e-9_V);
EXPECT_LE(units::math::abs(signal), (constraints.maxInput + 1e-9_V));
return next;
}
@@ -52,8 +52,8 @@ TEST(ExponentialProfileTest, ReachesGoal) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -69,8 +69,8 @@ TEST(ExponentialProfileTest, PosContinousUnderVelChange) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -92,8 +92,8 @@ TEST(ExponentialProfileTest, PosContinousUnderVelChangeBackward) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -114,8 +114,8 @@ TEST(ExponentialProfileTest, Backwards) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state;
@@ -129,8 +129,8 @@ TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -151,8 +151,8 @@ TEST(ExponentialProfileTest, TopSpeed) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state;
@@ -172,8 +172,8 @@ TEST(ExponentialProfileTest, TopSpeedBackward) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state;
@@ -193,8 +193,8 @@ TEST(ExponentialProfileTest, HighInitialSpeed) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 8_mps};
@@ -210,8 +210,8 @@ TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, -8_mps};
@@ -278,8 +278,8 @@ TEST(ExponentialProfileTest, TimingToCurrent) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -295,8 +295,8 @@ TEST(ExponentialProfileTest, TimingToGoal) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -318,8 +318,8 @@ TEST(ExponentialProfileTest, TimingToNegativeGoal) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-2_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};