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

[sysid] Show warning tooltips next to bad feedforward gains instead of throwing (#6251)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
HarryXChen
2024-01-19 23:33:56 -05:00
committed by GitHub
parent d392570659
commit 1330235918
8 changed files with 247 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

114
sysid/src/main/native/cpp/view/Analyzer.cpp
View File

@@ -48,10 +48,10 @@ Analyzer::Analyzer(glass::Storage& storage, wpi::Logger& logger)
void Analyzer::UpdateFeedforwardGains() {
WPI_INFO(m_logger, "{}", "Gain calc");
try {
const auto& [ff] = m_manager->CalculateFeedforward();
m_ff = ff.coeffs;
m_accelRSquared = ff.rSquared;
m_accelRMSE = ff.rmse;
const auto& feedforwardGains = m_manager->CalculateFeedforward();
m_feedforwardGains = feedforwardGains;
m_accelRSquared = feedforwardGains.olsResult.rSquared;
m_accelRMSE = feedforwardGains.olsResult.rmse;
m_settings.preset.measurementDelay =
m_settings.type == FeedbackControllerLoopType::kPosition
? m_manager->GetPositionDelay()
@@ -80,16 +80,22 @@ void Analyzer::UpdateFeedforwardGains() {
void Analyzer::UpdateFeedbackGains() {
WPI_INFO(m_logger, "{}", "Updating feedback gains");
if (m_ff[1] > 0 && m_ff[2] > 0) {
const auto& fb = m_manager->CalculateFeedback(m_ff);
m_timescale = units::second_t{m_ff[2] / m_ff[1]};
const auto& Kv = m_feedforwardGains.Kv;
const auto& Ka = m_feedforwardGains.Ka;
if (Kv.isValidGain && Ka.isValidGain) {
const auto& fb = m_manager->CalculateFeedback(Kv, Ka);
m_timescale = units::second_t{Ka.gain / Kv.gain};
m_timescaleValid = true;
m_Kp = fb.Kp;
m_Kd = fb.Kd;
} else {
m_timescaleValid = false;
}
}
bool Analyzer::DisplayGain(const char* text, double* data,
bool readOnly = true) {
bool Analyzer::DisplayDouble(const char* text, double* data,
bool readOnly = true) {
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 5);
if (readOnly) {
return ImGui::InputDouble(text, data, 0.0, 0.0, "%.5G",
@@ -121,7 +127,7 @@ bool Analyzer::IsDataErrorState() {
void Analyzer::ResetData() {
m_plot.ResetData();
m_manager = std::make_unique<AnalysisManager>(m_settings, m_logger);
m_ff = std::vector<double>{1, 1, 1};
m_feedforwardGains = AnalysisManager::FeedforwardGains{};
UpdateFeedbackGains();
}
@@ -182,7 +188,8 @@ void Analyzer::ConfigParamsOnFileSelect() {
// Estimate qp as 1/10 native distance unit
m_settings.lqr.qp = 0.1;
// Estimate qv as 1/4 * max velocity = 1/4 * (12V - kS) / kV
m_settings.lqr.qv = 0.25 * (12.0 - m_ff[0]) / m_ff[1];
m_settings.lqr.qv =
0.25 * (12.0 - m_feedforwardGains.Ks.gain) / m_feedforwardGains.Kv.gain;
}
void Analyzer::Display() {
@@ -302,8 +309,9 @@ void Analyzer::PrepareGraphs() {
AbortDataPrep();
m_dataThread = std::thread([&] {
m_plot.SetData(m_manager->GetRawData(), m_manager->GetFilteredData(),
m_manager->GetUnit(), m_ff, m_manager->GetStartTimes(),
m_manager->GetAnalysisType(), m_abortDataPrep);
m_manager->GetUnit(), m_feedforwardGains,
m_manager->GetStartTimes(), m_manager->GetAnalysisType(),
m_abortDataPrep);
});
UpdateFeedbackGains();
m_state = AnalyzerState::kNominalDisplay;
@@ -355,28 +363,28 @@ void Analyzer::DisplayGraphs() {
// If a JSON is selected
if (m_state == AnalyzerState::kNominalDisplay) {
DisplayGain("Acceleration R²", &m_accelRSquared);
DisplayDouble("Acceleration R²", &m_accelRSquared);
CreateTooltip(
"The coefficient of determination of the OLS fit of acceleration "
"versus velocity and voltage. Acceleration is extremely noisy, "
"so this is generally quite small.");
ImGui::SameLine();
DisplayGain("Acceleration RMSE", &m_accelRMSE);
DisplayDouble("Acceleration RMSE", &m_accelRMSE);
CreateTooltip(
"The standard deviation of the residuals from the predicted "
"acceleration."
"This can be interpreted loosely as the mean measured disturbance "
"from the \"ideal\" system equation.");
DisplayGain("Sim velocity R²", m_plot.GetSimRSquared());
DisplayDouble("Sim velocity R²", m_plot.GetSimRSquared());
CreateTooltip(
"The coefficient of determination the simulated velocity. "
"Velocity is much less-noisy than acceleration, so this "
"is pretty close to 1 for a decent fit.");
ImGui::SameLine();
DisplayGain("Sim velocity RMSE", m_plot.GetSimRMSE());
DisplayDouble("Sim velocity RMSE", m_plot.GetSimRMSE());
CreateTooltip(
"The standard deviation of the residuals from the simulated velocity "
"predictions - essentially the size of the mean error of the "
@@ -407,7 +415,7 @@ void Analyzer::AbortDataPrep() {
void Analyzer::DisplayFeedforwardParameters(float beginX, float beginY) {
// Increase spacing to not run into trackwidth in the normal analyzer view
constexpr double kHorizontalOffset = 0.9;
constexpr double kHorizontalOffset = 1.1;
SetPosition(beginX, beginY, kHorizontalOffset, 0);
bool displayAll =
@@ -457,20 +465,20 @@ void Analyzer::DisplayFeedforwardParameters(float beginX, float beginY) {
void Analyzer::CollectFeedforwardGains(float beginX, float beginY) {
SetPosition(beginX, beginY, 0, 0);
if (DisplayGain("Kv", &m_ff[1], false)) {
if (DisplayDouble("Kv", &m_feedforwardGains.Kv.gain, false)) {
UpdateFeedbackGains();
}
SetPosition(beginX, beginY, 0, 1);
if (DisplayGain("Ka", &m_ff[2], false)) {
if (DisplayDouble("Ka", &m_feedforwardGains.Ka.gain, false)) {
UpdateFeedbackGains();
}
SetPosition(beginX, beginY, 0, 2);
// Show Timescale
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
DisplayGain("Response Timescale (ms)",
reinterpret_cast<double*>(&m_timescale));
DisplayDouble("Response Timescale (ms)",
reinterpret_cast<double*>(&m_timescale));
CreateTooltip(
"The characteristic timescale of the system response in milliseconds. "
"Both the control loop period and total signal delay should be "
@@ -478,21 +486,39 @@ void Analyzer::CollectFeedforwardGains(float beginX, float beginY) {
"system.");
}
void Analyzer::DisplayFeedforwardGain(const char* text,
AnalysisManager::FeedforwardGain& ffGain,
bool readOnly = true) {
DisplayDouble(text, &ffGain.gain, readOnly);
if (!ffGain.isValidGain) {
// Display invalid gain message with warning and tooltip
CreateErrorTooltip(ffGain.errorMessage.c_str());
}
// Display descriptor message as tooltip, whether the gain is valid or not
CreateTooltip(ffGain.descriptor.c_str());
}
void Analyzer::DisplayFeedforwardGains(float beginX, float beginY) {
SetPosition(beginX, beginY, 0, 0);
DisplayGain("Ks", &m_ff[0]);
DisplayFeedforwardGain("Ks", m_feedforwardGains.Ks);
SetPosition(beginX, beginY, 0, 1);
DisplayGain("Kv", &m_ff[1]);
DisplayFeedforwardGain("Kv", m_feedforwardGains.Kv);
SetPosition(beginX, beginY, 0, 2);
DisplayGain("Ka", &m_ff[2]);
DisplayFeedforwardGain("Ka", m_feedforwardGains.Ka);
SetPosition(beginX, beginY, 0, 3);
// Show Timescale
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
DisplayGain("Response Timescale (ms)",
reinterpret_cast<double*>(&m_timescale));
DisplayDouble("Response Timescale (ms)",
reinterpret_cast<double*>(&m_timescale));
if (!m_timescaleValid) {
CreateErrorTooltip(
"Response timescale calculation invalid. Ensure that calculated gains "
"are valid.");
}
CreateTooltip(
"The characteristic timescale of the system response in milliseconds. "
"Both the control loop period and total signal delay should be "
@@ -501,8 +527,8 @@ void Analyzer::DisplayFeedforwardGains(float beginX, float beginY) {
SetPosition(beginX, beginY, 0, 4);
auto positionDelay = m_manager->GetPositionDelay();
DisplayGain("Position Measurement Delay (ms)",
reinterpret_cast<double*>(&positionDelay));
DisplayDouble("Position Measurement Delay (ms)",
reinterpret_cast<double*>(&positionDelay));
CreateTooltip(
"The average elapsed time between the first application of "
"voltage and the first detected change in mechanism position "
@@ -512,8 +538,8 @@ void Analyzer::DisplayFeedforwardGains(float beginX, float beginY) {
SetPosition(beginX, beginY, 0, 5);
auto velocityDelay = m_manager->GetVelocityDelay();
DisplayGain("Velocity Measurement Delay (ms)",
reinterpret_cast<double*>(&velocityDelay));
DisplayDouble("Velocity Measurement Delay (ms)",
reinterpret_cast<double*>(&velocityDelay));
CreateTooltip(
"The average elapsed time between the first application of "
"voltage and the maximum calculated mechanism acceleration "
@@ -524,20 +550,20 @@ void Analyzer::DisplayFeedforwardGains(float beginX, float beginY) {
SetPosition(beginX, beginY, 0, 6);
if (m_manager->GetAnalysisType() == analysis::kElevator) {
DisplayGain("Kg", &m_ff[3]);
DisplayFeedforwardGain("Kg", m_feedforwardGains.Kg);
} else if (m_manager->GetAnalysisType() == analysis::kArm) {
DisplayGain("Kg", &m_ff[3]);
DisplayFeedforwardGain("Kg", m_feedforwardGains.Kg);
double offset;
auto unit = m_manager->GetUnit();
if (unit == "Radians") {
offset = m_ff[4];
offset = m_feedforwardGains.offset.gain;
} else if (unit == "Degrees") {
offset = m_ff[4] / std::numbers::pi * 180.0;
offset = m_feedforwardGains.offset.gain / std::numbers::pi * 180.0;
} else if (unit == "Rotations") {
offset = m_ff[4] / (2 * std::numbers::pi);
offset = m_feedforwardGains.offset.gain / (2 * std::numbers::pi);
}
DisplayGain(
DisplayDouble(
fmt::format("Angle offset to horizontal ({})", GetAbbreviation(unit))
.c_str(),
&offset);
@@ -671,10 +697,10 @@ void Analyzer::DisplayFeedbackGains() {
// Show Kp and Kd.
float beginY = ImGui::GetCursorPosY();
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
DisplayGain("Kp", &m_Kp);
DisplayDouble("Kp", &m_Kp);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
DisplayGain("Kd", &m_Kd);
DisplayDouble("Kd", &m_Kd);
// Come back to the starting y pos.
ImGui::SetCursorPosY(beginY);
@@ -686,8 +712,8 @@ void Analyzer::DisplayFeedbackGains() {
}
ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
if (DisplayGain(fmt::format("Max Position Error{}", unit).c_str(),
&m_settings.lqr.qp, false)) {
if (DisplayDouble(fmt::format("Max Position Error{}", unit).c_str(),
&m_settings.lqr.qp, false)) {
if (m_settings.lqr.qp > 0) {
UpdateFeedbackGains();
}
@@ -700,14 +726,14 @@ void Analyzer::DisplayFeedbackGains() {
}
ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
if (DisplayGain(fmt::format("Max Velocity Error{}", unit).c_str(),
&m_settings.lqr.qv, false)) {
if (DisplayDouble(fmt::format("Max Velocity Error{}", unit).c_str(),
&m_settings.lqr.qv, false)) {
if (m_settings.lqr.qv > 0) {
UpdateFeedbackGains();
}
}
ImGui::SetCursorPosX(ImGui::GetFontSize() * 9);
if (DisplayGain("Max Control Effort (V)", &m_settings.lqr.r, false)) {
if (DisplayDouble("Max Control Effort (V)", &m_settings.lqr.r, false)) {
if (m_settings.lqr.r > 0) {
UpdateFeedbackGains();
}