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

[wpimath] Fix another infinite loop in ArmFeedforward (#7823)

Browse Source
This commit is contained in:
Tyler Veness
2025-02-25 19:04:31 -08:00
committed by GitHub
parent b0e588fd49
commit 517344fe80
3 changed files with 186 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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);
}