Unify y2022 localizer into y2022/localizer folder
Somehow ended up creating multiple folders....
Change-Id: Ib8e7e69a7583379caecb9d0639042c77bba039ed
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/control_loops/drivetrain/BUILD b/y2022/control_loops/drivetrain/BUILD
index 5381254..3442a39 100644
--- a/y2022/control_loops/drivetrain/BUILD
+++ b/y2022/control_loops/drivetrain/BUILD
@@ -80,7 +80,7 @@
"//aos/network:message_bridge_server_fbs",
"//frc971/control_loops/drivetrain:hybrid_ekf",
"//frc971/control_loops/drivetrain:localizer",
- "//y2022/control_loops/localizer:localizer_output_fbs",
+ "//y2022/localizer:localizer_output_fbs",
],
)
@@ -127,7 +127,7 @@
"//frc971/control_loops:team_number_test_environment",
"//frc971/control_loops/drivetrain:drivetrain_lib",
"//frc971/control_loops/drivetrain:drivetrain_test_lib",
- "//y2022/control_loops/localizer:localizer_output_fbs",
+ "//y2022/localizer:localizer_output_fbs",
],
)
diff --git a/y2022/control_loops/drivetrain/localizer.h b/y2022/control_loops/drivetrain/localizer.h
index 50d083a..4505e9b 100644
--- a/y2022/control_loops/drivetrain/localizer.h
+++ b/y2022/control_loops/drivetrain/localizer.h
@@ -6,7 +6,7 @@
#include "aos/events/event_loop.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/drivetrain/localizer.h"
-#include "y2022/control_loops/localizer/localizer_output_generated.h"
+#include "y2022/localizer/localizer_output_generated.h"
#include "aos/network/message_bridge_server_generated.h"
namespace y2022 {
@@ -16,7 +16,7 @@
// This class handles the localization for the 2022 robot. Rather than actually
// doing any work on the roborio, we farm all the localization out to a
// raspberry pi and it then sends out LocalizerOutput messages that we treat as
-// measurement updates. See //y2022/control_loops/localizer.
+// measurement updates. See //y2022/localizer.
// TODO(james): Needs tests. Should refactor out some of the code from the 2020
// localizer test.
class Localizer : public frc971::control_loops::drivetrain::LocalizerInterface {
diff --git a/y2022/control_loops/drivetrain/localizer_test.cc b/y2022/control_loops/drivetrain/localizer_test.cc
index 87fc3fd..1e33826 100644
--- a/y2022/control_loops/drivetrain/localizer_test.cc
+++ b/y2022/control_loops/drivetrain/localizer_test.cc
@@ -10,7 +10,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/team_number_test_environment.h"
-#include "y2022/control_loops/localizer/localizer_output_generated.h"
+#include "y2022/localizer/localizer_output_generated.h"
#include "gtest/gtest.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
diff --git a/y2022/control_loops/localizer/BUILD b/y2022/control_loops/localizer/BUILD
deleted file mode 100644
index f1bc2d1..0000000
--- a/y2022/control_loops/localizer/BUILD
+++ /dev/null
@@ -1,121 +0,0 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
-load("@npm//@bazel/typescript:index.bzl", "ts_library")
-
-ts_library(
- name = "localizer_plotter",
- srcs = ["localizer_plotter.ts"],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//aos/network/www:aos_plotter",
- "//aos/network/www:colors",
- "//aos/network/www:proxy",
- "//frc971/wpilib:imu_plot_utils",
- ],
-)
-
-flatbuffer_cc_library(
- name = "localizer_output_fbs",
- srcs = [
- "localizer_output.fbs",
- ],
- gen_reflections = True,
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
-)
-
-flatbuffer_cc_library(
- name = "localizer_status_fbs",
- srcs = [
- "localizer_status.fbs",
- ],
- gen_reflections = True,
- includes = [
- "//frc971/control_loops:control_loops_fbs_includes",
- "//frc971/control_loops/drivetrain:drivetrain_status_fbs_includes",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
-)
-
-cc_static_flatbuffer(
- name = "localizer_schema",
- function = "frc971::controls::LocalizerStatusSchema",
- target = ":localizer_status_fbs_reflection_out",
- visibility = ["//visibility:public"],
-)
-
-cc_library(
- name = "localizer",
- srcs = ["localizer.cc"],
- hdrs = ["localizer.h"],
- visibility = ["//visibility:public"],
- deps = [
- ":localizer_output_fbs",
- ":localizer_status_fbs",
- "//aos/containers:ring_buffer",
- "//aos/events:event_loop",
- "//aos/time",
- "//frc971/control_loops:c2d",
- "//frc971/control_loops:control_loops_fbs",
- "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
- "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
- "//frc971/control_loops/drivetrain:improved_down_estimator",
- "//frc971/control_loops/drivetrain:localizer_fbs",
- "//frc971/wpilib:imu_batch_fbs",
- "//frc971/wpilib:imu_fbs",
- "//frc971/zeroing:imu_zeroer",
- "//frc971/zeroing:wrap",
- "//y2020/vision/sift:sift_fbs",
- "//y2022:constants",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_binary(
- name = "localizer_main",
- srcs = ["localizer_main.cc"],
- visibility = ["//visibility:public"],
- deps = [
- ":localizer",
- "//aos:init",
- "//aos/events:shm_event_loop",
- "//y2022/control_loops/drivetrain:drivetrain_base",
- ],
-)
-
-cc_test(
- name = "localizer_test",
- srcs = ["localizer_test.cc"],
- data = [
- "//y2022:aos_config",
- ],
- shard_count = 10,
- deps = [
- ":localizer",
- "//aos/events:simulated_event_loop",
- "//aos/testing:googletest",
- "//frc971/control_loops/drivetrain:drivetrain_test_lib",
- ],
-)
-
-cc_binary(
- name = "localizer_replay",
- srcs = ["localizer_replay.cc"],
- data = [
- "//y2022:aos_config",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":localizer",
- ":localizer_schema",
- "//aos:configuration",
- "//aos:init",
- "//aos:json_to_flatbuffer",
- "//aos/events:simulated_event_loop",
- "//aos/events/logging:log_reader",
- "//aos/events/logging:log_writer",
- "//y2022/control_loops/drivetrain:drivetrain_base",
- ],
-)
diff --git a/y2022/control_loops/localizer/localizer.cc b/y2022/control_loops/localizer/localizer.cc
deleted file mode 100644
index 56c2769..0000000
--- a/y2022/control_loops/localizer/localizer.cc
+++ /dev/null
@@ -1,629 +0,0 @@
-#include "y2022/control_loops/localizer/localizer.h"
-
-#include "frc971/control_loops/c2d.h"
-#include "frc971/wpilib/imu_batch_generated.h"
-#include "y2022/constants.h"
-
-namespace frc971::controls {
-
-namespace {
-constexpr double kG = 9.80665;
-constexpr std::chrono::microseconds kNominalDt(500);
-
-template <int N>
-Eigen::Matrix<double, N, 1> MakeState(std::vector<double> values) {
- CHECK_EQ(static_cast<size_t>(N), values.size());
- Eigen::Matrix<double, N, 1> vector;
- for (int ii = 0; ii < N; ++ii) {
- vector(ii, 0) = values[ii];
- }
- return vector;
-}
-} // namespace
-
-ModelBasedLocalizer::ModelBasedLocalizer(
- const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
- : dt_config_(dt_config),
- velocity_drivetrain_coefficients_(
- dt_config.make_hybrid_drivetrain_velocity_loop()
- .plant()
- .coefficients()),
- down_estimator_(dt_config) {
- CHECK_EQ(branches_.capacity(), static_cast<size_t>(std::chrono::seconds(1) /
- kNominalDt / kBranchPeriod));
- if (dt_config_.is_simulated) {
- down_estimator_.assume_perfect_gravity();
- }
- A_continuous_accel_.setZero();
- A_continuous_model_.setZero();
- B_continuous_accel_.setZero();
- B_continuous_model_.setZero();
-
- A_continuous_accel_(kX, kVelocityX) = 1.0;
- A_continuous_accel_(kY, kVelocityY) = 1.0;
-
- const double diameter = 2.0 * dt_config_.robot_radius;
-
- A_continuous_model_(kTheta, kLeftVelocity) = -1.0 / diameter;
- A_continuous_model_(kTheta, kRightVelocity) = 1.0 / diameter;
- A_continuous_model_(kLeftEncoder, kLeftVelocity) = 1.0;
- A_continuous_model_(kRightEncoder, kRightVelocity) = 1.0;
- const auto &vel_coefs = velocity_drivetrain_coefficients_;
- A_continuous_model_(kLeftVelocity, kLeftVelocity) =
- vel_coefs.A_continuous(0, 0);
- A_continuous_model_(kLeftVelocity, kRightVelocity) =
- vel_coefs.A_continuous(0, 1);
- A_continuous_model_(kRightVelocity, kLeftVelocity) =
- vel_coefs.A_continuous(1, 0);
- A_continuous_model_(kRightVelocity, kRightVelocity) =
- vel_coefs.A_continuous(1, 1);
-
- A_continuous_model_(kLeftVelocity, kLeftVoltageError) =
- 1 * vel_coefs.B_continuous(0, 0);
- A_continuous_model_(kLeftVelocity, kRightVoltageError) =
- 1 * vel_coefs.B_continuous(0, 1);
- A_continuous_model_(kRightVelocity, kLeftVoltageError) =
- 1 * vel_coefs.B_continuous(1, 0);
- A_continuous_model_(kRightVelocity, kRightVoltageError) =
- 1 * vel_coefs.B_continuous(1, 1);
-
- B_continuous_model_.block<1, 2>(kLeftVelocity, kLeftVoltage) =
- vel_coefs.B_continuous.row(0);
- B_continuous_model_.block<1, 2>(kRightVelocity, kLeftVoltage) =
- vel_coefs.B_continuous.row(1);
-
- B_continuous_accel_(kVelocityX, kAccelX) = 1.0;
- B_continuous_accel_(kVelocityY, kAccelY) = 1.0;
- B_continuous_accel_(kTheta, kThetaRate) = 1.0;
-
- Q_continuous_model_.setZero();
- Q_continuous_model_.diagonal() << 1e-4, 1e-4, 1e-4, 1e-2, 1e-0, 1e-0, 1e-2,
- 1e-0, 1e-0;
-
- P_model_ = Q_continuous_model_ * aos::time::DurationInSeconds(kNominalDt);
-}
-
-Eigen::Matrix<double, ModelBasedLocalizer::kNModelStates,
- ModelBasedLocalizer::kNModelStates>
-ModelBasedLocalizer::AModel(
- const ModelBasedLocalizer::ModelState &state) const {
- Eigen::Matrix<double, kNModelStates, kNModelStates> A = A_continuous_model_;
- const double theta = state(kTheta);
- const double stheta = std::sin(theta);
- const double ctheta = std::cos(theta);
- const double velocity = (state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
- A(kX, kTheta) = -stheta * velocity;
- A(kX, kLeftVelocity) = ctheta / 2.0;
- A(kX, kRightVelocity) = ctheta / 2.0;
- A(kY, kTheta) = ctheta * velocity;
- A(kY, kLeftVelocity) = stheta / 2.0;
- A(kY, kRightVelocity) = stheta / 2.0;
- return A;
-}
-
-Eigen::Matrix<double, ModelBasedLocalizer::kNAccelStates,
- ModelBasedLocalizer::kNAccelStates>
-ModelBasedLocalizer::AAccel() const {
- return A_continuous_accel_;
-}
-
-ModelBasedLocalizer::ModelState ModelBasedLocalizer::DiffModel(
- const ModelBasedLocalizer::ModelState &state,
- const ModelBasedLocalizer::ModelInput &U) const {
- ModelState x_dot = AModel(state) * state + B_continuous_model_ * U;
- const double theta = state(kTheta);
- const double stheta = std::sin(theta);
- const double ctheta = std::cos(theta);
- const double velocity = (state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
- x_dot(kX) = ctheta * velocity;
- x_dot(kY) = stheta * velocity;
- return x_dot;
-}
-
-ModelBasedLocalizer::AccelState ModelBasedLocalizer::DiffAccel(
- const ModelBasedLocalizer::AccelState &state,
- const ModelBasedLocalizer::AccelInput &U) const {
- return AAccel() * state + B_continuous_accel_ * U;
-}
-
-ModelBasedLocalizer::ModelState ModelBasedLocalizer::UpdateModel(
- const ModelBasedLocalizer::ModelState &model,
- const ModelBasedLocalizer::ModelInput &input,
- const aos::monotonic_clock::duration dt) const {
- return control_loops::RungeKutta(
- std::bind(&ModelBasedLocalizer::DiffModel, this, std::placeholders::_1,
- input),
- model, aos::time::DurationInSeconds(dt));
-}
-
-ModelBasedLocalizer::AccelState ModelBasedLocalizer::UpdateAccel(
- const ModelBasedLocalizer::AccelState &accel,
- const ModelBasedLocalizer::AccelInput &input,
- const aos::monotonic_clock::duration dt) const {
- return control_loops::RungeKutta(
- std::bind(&ModelBasedLocalizer::DiffAccel, this, std::placeholders::_1,
- input),
- accel, aos::time::DurationInSeconds(dt));
-}
-
-ModelBasedLocalizer::AccelState ModelBasedLocalizer::AccelStateForModelState(
- const ModelBasedLocalizer::ModelState &state) const {
- const double robot_speed =
- (state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
- const double lat_speed = (AModel(state) * state)(kTheta) * long_offset_;
- const double velocity_x = std::cos(state(kTheta)) * robot_speed -
- std::sin(state(kTheta)) * lat_speed;
- const double velocity_y = std::sin(state(kTheta)) * robot_speed +
- std::cos(state(kTheta)) * lat_speed;
- return (AccelState() << state(0), state(1), state(2), velocity_x, velocity_y)
- .finished();
-}
-
-ModelBasedLocalizer::ModelState ModelBasedLocalizer::ModelStateForAccelState(
- const ModelBasedLocalizer::AccelState &state,
- const Eigen::Vector2d &encoders, const double yaw_rate) const {
- const double robot_speed = state(kVelocityX) * std::cos(state(kTheta)) +
- state(kVelocityY) * std::sin(state(kTheta));
- const double radius = dt_config_.robot_radius;
- const double left_velocity = robot_speed - yaw_rate * radius;
- const double right_velocity = robot_speed + yaw_rate * radius;
- return (ModelState() << state(0), state(1), state(2), encoders(0),
- left_velocity, 0.0, encoders(1), right_velocity, 0.0)
- .finished();
-}
-
-double ModelBasedLocalizer::ModelDivergence(
- const ModelBasedLocalizer::CombinedState &state,
- const ModelBasedLocalizer::AccelInput &accel_inputs,
- const Eigen::Vector2d &filtered_accel,
- const ModelBasedLocalizer::ModelInput &model_inputs) {
- // Convert the model state into the acceleration-based state-space and check
- // the distance between the two (should really be a weighted norm, but all the
- // numbers are on ~the same scale).
- // TODO(james): Maybe weight lateral velocity divergence different than
- // longitudinal? Seems like we tend to get false-positives currently when in
- // sharp turns.
- // TODO(james): For off-center gyros, maybe reduce noise when turning?
- VLOG(2) << "divergence: "
- << (state.accel_state - AccelStateForModelState(state.model_state))
- .transpose();
- const AccelState diff_accel = DiffAccel(state.accel_state, accel_inputs);
- const ModelState diff_model = DiffModel(state.model_state, model_inputs);
- const double model_lng_velocity =
- (state.model_state(kLeftVelocity) + state.model_state(kRightVelocity)) /
- 2.0;
- const double model_lng_accel =
- (diff_model(kLeftVelocity) + diff_model(kRightVelocity)) / 2.0 -
- diff_model(kTheta) * diff_model(kTheta) * long_offset_;
- const double model_lat_accel = diff_model(kTheta) * model_lng_velocity;
- const Eigen::Vector2d robot_frame_accel(model_lng_accel, model_lat_accel);
- const Eigen::Vector2d model_accel =
- Eigen::AngleAxisd(state.model_state(kTheta), Eigen::Vector3d::UnitZ())
- .toRotationMatrix()
- .block<2, 2>(0, 0) *
- robot_frame_accel;
- const double accel_diff = (model_accel - filtered_accel).norm();
- const double theta_rate_diff =
- std::abs(diff_accel(kTheta) - diff_model(kTheta));
-
- const Eigen::Vector2d accel_vel = state.accel_state.bottomRows<2>();
- Eigen::Vector2d model_vel =
- AccelStateForModelState(state.model_state).bottomRows<2>();
- velocity_residual_ = (accel_vel - model_vel).norm() /
- (1.0 + accel_vel.norm() + model_vel.norm());
- theta_rate_residual_ = theta_rate_diff;
- accel_residual_ = accel_diff / 4.0;
- return velocity_residual_ + theta_rate_residual_ + accel_residual_;
-}
-
-void ModelBasedLocalizer::UpdateState(
- CombinedState *state,
- const Eigen::Matrix<double, kNModelStates, kNModelOutputs> &K,
- const Eigen::Matrix<double, kNModelOutputs, 1> &Z,
- const Eigen::Matrix<double, kNModelOutputs, kNModelStates> &H,
- const AccelInput &accel_input, const ModelInput &model_input,
- aos::monotonic_clock::duration dt) {
- state->accel_state = UpdateAccel(state->accel_state, accel_input, dt);
- if (down_estimator_.consecutive_still() > 500.0) {
- state->accel_state(kVelocityX) *= 0.9;
- state->accel_state(kVelocityY) *= 0.9;
- }
- state->model_state = UpdateModel(state->model_state, model_input, dt);
- state->model_state += K * (Z - H * state->model_state);
-}
-
-void ModelBasedLocalizer::HandleImu(aos::monotonic_clock::time_point t,
- const Eigen::Vector3d &gyro,
- const Eigen::Vector3d &accel,
- const Eigen::Vector2d encoders,
- const Eigen::Vector2d voltage) {
- VLOG(2) << t;
- if (t_ == aos::monotonic_clock::min_time) {
- t_ = t;
- }
- if (t_ + 2 * kNominalDt < t) {
- t_ = t;
- ++clock_resets_;
- }
- const aos::monotonic_clock::duration dt = t - t_;
- t_ = t;
- down_estimator_.Predict(gyro, accel, dt);
- // TODO(james): Should we prefer this or use the down-estimator corrected
- // version? Using the down estimator is more principled, but does create more
- // opportunities for subtle biases.
- const double yaw_rate = (dt_config_.imu_transform * gyro)(2);
- const double diameter = 2.0 * dt_config_.robot_radius;
-
- const Eigen::AngleAxis<double> orientation(
- Eigen::AngleAxis<double>(xytheta()(kTheta), Eigen::Vector3d::UnitZ()) *
- down_estimator_.X_hat());
- last_orientation_ = orientation;
-
- const Eigen::Vector3d absolute_accel =
- orientation * dt_config_.imu_transform * kG * accel;
- abs_accel_ = absolute_accel;
-
- VLOG(2) << "abs accel " << absolute_accel.transpose();
- VLOG(2) << "dt " << aos::time::DurationInSeconds(dt);
-
- // Update all the branched states.
- const AccelInput accel_input(absolute_accel.x(), absolute_accel.y(),
- yaw_rate);
- const ModelInput model_input(voltage);
-
- const Eigen::Matrix<double, kNModelStates, kNModelStates> A_continuous =
- AModel(current_state_.model_state);
-
- Eigen::Matrix<double, kNModelStates, kNModelStates> A_discrete;
- Eigen::Matrix<double, kNModelStates, kNModelStates> Q_discrete;
-
- DiscretizeQAFast(Q_continuous_model_, A_continuous, dt, &Q_discrete,
- &A_discrete);
-
- P_model_ = A_discrete * P_model_ * A_discrete.transpose() + Q_discrete;
-
- Eigen::Matrix<double, kNModelOutputs, kNModelStates> H;
- Eigen::Matrix<double, kNModelOutputs, kNModelOutputs> R;
- {
- H.setZero();
- R.setZero();
- H(0, kLeftEncoder) = 1.0;
- H(1, kRightEncoder) = 1.0;
- H(2, kRightVelocity) = 1.0 / diameter;
- H(2, kLeftVelocity) = -1.0 / diameter;
-
- R.diagonal() << 1e-9, 1e-9, 1e-13;
- }
-
- const Eigen::Matrix<double, kNModelOutputs, 1> Z(encoders(0), encoders(1),
- yaw_rate);
-
- if (branches_.empty()) {
- VLOG(2) << "Initializing";
- current_state_.model_state.setZero();
- current_state_.accel_state.setZero();
- current_state_.model_state(kLeftEncoder) = encoders(0);
- current_state_.model_state(kRightEncoder) = encoders(1);
- current_state_.branch_time = t;
- branches_.Push(current_state_);
- }
-
- const Eigen::Matrix<double, kNModelStates, kNModelOutputs> K =
- P_model_ * H.transpose() * (H * P_model_ * H.transpose() + R).inverse();
- P_model_ = (Eigen::Matrix<double, kNModelStates, kNModelStates>::Identity() -
- K * H) *
- P_model_;
- VLOG(2) << "K\n" << K;
- VLOG(2) << "Z\n" << Z.transpose();
-
- for (CombinedState &state : branches_) {
- UpdateState(&state, K, Z, H, accel_input, model_input, dt);
- }
- UpdateState(¤t_state_, K, Z, H, accel_input, model_input, dt);
-
- VLOG(2) << "oildest accel " << branches_[0].accel_state.transpose();
- VLOG(2) << "oildest accel diff "
- << DiffAccel(branches_[0].accel_state, accel_input).transpose();
- VLOG(2) << "oildest model " << branches_[0].model_state.transpose();
-
- // Determine whether to switch modes--if we are currently in model-based mode,
- // swap to accel-based if the two states have divergeed meaningfully in the
- // oldest branch. If we are currently in accel-based, then swap back to model
- // if the oldest model branch matches has matched the
- filtered_residual_accel_ +=
- 0.01 * (accel_input.topRows<2>() - filtered_residual_accel_);
- const double model_divergence =
- branches_.full() ? ModelDivergence(branches_[0], accel_input,
- filtered_residual_accel_, model_input)
- : 0.0;
- filtered_residual_ +=
- (1.0 - std::exp(-aos::time::DurationInSeconds(kNominalDt) / 0.0095)) *
- (model_divergence - filtered_residual_);
- // TODO(james): Tune this more. Currently set to generally trust the model,
- // perhaps a bit too much.
- // When the residual exceeds the accel threshold, we start using the inertials
- // alone; when it drops back below the model threshold, we go back to being
- // model-based.
- constexpr double kUseAccelThreshold = 2.0;
- constexpr double kUseModelThreshold = 0.5;
- constexpr size_t kShareStates = kNModelStates;
- static_assert(kUseModelThreshold < kUseAccelThreshold);
- if (using_model_) {
- if (filtered_residual_ > kUseAccelThreshold) {
- hysteresis_count_++;
- } else {
- hysteresis_count_ = 0;
- }
- if (hysteresis_count_ > 0) {
- using_model_ = false;
- // Grab the accel-based state from back when we started diverging.
- // TODO(james): This creates a problematic selection bias, because
- // we will tend to bias towards deliberately out-of-tune measurements.
- current_state_.accel_state = branches_[0].accel_state;
- current_state_.model_state = branches_[0].model_state;
- current_state_.model_state = ModelStateForAccelState(
- current_state_.accel_state, encoders, yaw_rate);
- } else {
- VLOG(2) << "Normal branching";
- current_state_.accel_state =
- AccelStateForModelState(current_state_.model_state);
- current_state_.branch_time = t;
- }
- hysteresis_count_ = 0;
- } else {
- if (filtered_residual_ < kUseModelThreshold) {
- hysteresis_count_++;
- } else {
- hysteresis_count_ = 0;
- }
- if (hysteresis_count_ > 100) {
- using_model_ = true;
- // Grab the model-based state from back when we stopped diverging.
- current_state_.model_state.topRows<kShareStates>() =
- ModelStateForAccelState(branches_[0].accel_state, encoders, yaw_rate)
- .topRows<kShareStates>();
- current_state_.accel_state =
- AccelStateForModelState(current_state_.model_state);
- } else {
- // TODO(james): Why was I leaving the encoders/wheel velocities in place?
- current_state_.model_state = ModelStateForAccelState(
- current_state_.accel_state, encoders, yaw_rate);
- current_state_.branch_time = t;
- }
- }
-
- // Generate a new branch, with the accel state reset based on the model-based
- // state (really, just getting rid of the lateral velocity).
- // By resetting the accel state in the new branch, this tries to minimize the
- // odds of runaway lateral velocities. This doesn't help with runaway
- // longitudinal velocities, however.
- CombinedState new_branch = current_state_;
- new_branch.accel_state = AccelStateForModelState(new_branch.model_state);
- new_branch.accumulated_divergence = 0.0;
-
- ++branch_counter_;
- if (branch_counter_ % kBranchPeriod == 0) {
- branches_.Push(new_branch);
- branch_counter_ = 0;
- }
-
- last_residual_ = model_divergence;
-
- VLOG(2) << "Using " << (using_model_ ? "model" : "accel");
- VLOG(2) << "Residual " << last_residual_;
- VLOG(2) << "Filtered Residual " << filtered_residual_;
- VLOG(2) << "buffer size " << branches_.size();
- VLOG(2) << "Model state " << current_state_.model_state.transpose();
- VLOG(2) << "Accel state " << current_state_.accel_state.transpose();
- VLOG(2) << "Accel state for model "
- << AccelStateForModelState(current_state_.model_state).transpose();
- VLOG(2) << "Input acce " << accel.transpose();
- VLOG(2) << "Input gyro " << gyro.transpose();
- VLOG(2) << "Input voltage " << voltage.transpose();
- VLOG(2) << "Input encoder " << encoders.transpose();
- VLOG(2) << "yaw rate " << yaw_rate;
-
- CHECK(std::isfinite(last_residual_));
-}
-
-void ModelBasedLocalizer::HandleReset(aos::monotonic_clock::time_point now,
- const Eigen::Vector3d &xytheta) {
- branches_.Reset();
- t_ = now;
- using_model_ = true;
- current_state_.model_state << xytheta(0), xytheta(1), xytheta(2),
- current_state_.model_state(kLeftEncoder), 0.0, 0.0,
- current_state_.model_state(kRightEncoder), 0.0, 0.0;
- current_state_.accel_state =
- AccelStateForModelState(current_state_.model_state);
- last_residual_ = 0.0;
- filtered_residual_ = 0.0;
- filtered_residual_accel_.setZero();
- abs_accel_.setZero();
-}
-
-flatbuffers::Offset<AccelBasedState> ModelBasedLocalizer::BuildAccelState(
- flatbuffers::FlatBufferBuilder *fbb, const AccelState &state) {
- AccelBasedState::Builder accel_state_builder(*fbb);
- accel_state_builder.add_x(state(kX));
- accel_state_builder.add_y(state(kY));
- accel_state_builder.add_theta(state(kTheta));
- accel_state_builder.add_velocity_x(state(kVelocityX));
- accel_state_builder.add_velocity_y(state(kVelocityY));
- return accel_state_builder.Finish();
-}
-
-flatbuffers::Offset<ModelBasedState> ModelBasedLocalizer::BuildModelState(
- flatbuffers::FlatBufferBuilder *fbb, const ModelState &state) {
- ModelBasedState::Builder model_state_builder(*fbb);
- model_state_builder.add_x(state(kX));
- model_state_builder.add_y(state(kY));
- model_state_builder.add_theta(state(kTheta));
- model_state_builder.add_left_encoder(state(kLeftEncoder));
- model_state_builder.add_left_velocity(state(kLeftVelocity));
- model_state_builder.add_left_voltage_error(state(kLeftVoltageError));
- model_state_builder.add_right_encoder(state(kRightEncoder));
- model_state_builder.add_right_velocity(state(kRightVelocity));
- model_state_builder.add_right_voltage_error(state(kRightVoltageError));
- return model_state_builder.Finish();
-}
-
-flatbuffers::Offset<ModelBasedStatus> ModelBasedLocalizer::PopulateStatus(
- flatbuffers::FlatBufferBuilder *fbb) {
- const flatbuffers::Offset<control_loops::drivetrain::DownEstimatorState>
- down_estimator_offset = down_estimator_.PopulateStatus(fbb, t_);
-
- const CombinedState &state = current_state_;
-
- const flatbuffers::Offset<ModelBasedState> model_state_offset =
- BuildModelState(fbb, state.model_state);
-
- const flatbuffers::Offset<AccelBasedState> accel_state_offset =
- BuildAccelState(fbb, state.accel_state);
-
- const flatbuffers::Offset<AccelBasedState> oldest_accel_state_offset =
- branches_.empty() ? flatbuffers::Offset<AccelBasedState>()
- : BuildAccelState(fbb, branches_[0].accel_state);
-
- const flatbuffers::Offset<ModelBasedState> oldest_model_state_offset =
- branches_.empty() ? flatbuffers::Offset<ModelBasedState>()
- : BuildModelState(fbb, branches_[0].model_state);
-
- ModelBasedStatus::Builder builder(*fbb);
- builder.add_accel_state(accel_state_offset);
- builder.add_oldest_accel_state(oldest_accel_state_offset);
- builder.add_oldest_model_state(oldest_model_state_offset);
- builder.add_model_state(model_state_offset);
- builder.add_using_model(using_model_);
- builder.add_residual(last_residual_);
- builder.add_filtered_residual(filtered_residual_);
- builder.add_velocity_residual(velocity_residual_);
- builder.add_accel_residual(accel_residual_);
- builder.add_theta_rate_residual(theta_rate_residual_);
- builder.add_down_estimator(down_estimator_offset);
- builder.add_x(xytheta()(0));
- builder.add_y(xytheta()(1));
- builder.add_theta(xytheta()(2));
- builder.add_implied_accel_x(abs_accel_(0));
- builder.add_implied_accel_y(abs_accel_(1));
- builder.add_implied_accel_z(abs_accel_(2));
- builder.add_clock_resets(clock_resets_);
- return builder.Finish();
-}
-
-namespace {
-// Period at which the encoder readings from the IMU board wrap.
-static double DrivetrainWrapPeriod() {
- return y2022::constants::Values::DrivetrainEncoderToMeters(1 << 16);
-}
-}
-
-EventLoopLocalizer::EventLoopLocalizer(
- aos::EventLoop *event_loop,
- const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
- : event_loop_(event_loop),
- model_based_(dt_config),
- status_sender_(event_loop_->MakeSender<LocalizerStatus>("/localizer")),
- output_sender_(event_loop_->MakeSender<LocalizerOutput>("/localizer")),
- output_fetcher_(
- event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
- "/drivetrain")),
- left_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()),
- right_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()) {
- event_loop_->MakeWatcher(
- "/drivetrain",
- [this](
- const frc971::control_loops::drivetrain::LocalizerControl &control) {
- const double theta = control.keep_current_theta()
- ? model_based_.xytheta()(2)
- : control.theta();
- model_based_.HandleReset(event_loop_->monotonic_now(),
- {control.x(), control.y(), theta});
- });
- event_loop_->MakeWatcher(
- "/localizer", [this](const frc971::IMUValuesBatch &values) {
- CHECK(values.has_readings());
- output_fetcher_.Fetch();
- for (const IMUValues *value : *values.readings()) {
- zeroer_.InsertAndProcessMeasurement(*value);
- const Eigen::Vector2d encoders{
- left_encoder_.Unwrap(value->left_encoder()),
- right_encoder_.Unwrap(value->right_encoder())};
- if (zeroer_.Zeroed()) {
- const aos::monotonic_clock::time_point pico_timestamp{
- std::chrono::microseconds(value->pico_timestamp_us())};
- // TODO(james): If we get large enough drift off of the pico,
- // actually do something about it.
- if (!pico_offset_.has_value()) {
- pico_offset_ =
- event_loop_->context().monotonic_event_time - pico_timestamp;
- last_pico_timestamp_ = pico_timestamp;
- }
- if (pico_timestamp < last_pico_timestamp_) {
- pico_offset_.value() += std::chrono::microseconds(1ULL << 32);
- }
- const aos::monotonic_clock::time_point sample_timestamp =
- pico_offset_.value() + pico_timestamp;
- pico_offset_error_ =
- event_loop_->context().monotonic_event_time - sample_timestamp;
- const bool disabled =
- (output_fetcher_.get() == nullptr) ||
- (output_fetcher_.context().monotonic_event_time +
- std::chrono::milliseconds(10) <
- event_loop_->context().monotonic_event_time);
- model_based_.HandleImu(
- sample_timestamp,
- zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(), encoders,
- disabled ? Eigen::Vector2d::Zero()
- : Eigen::Vector2d{output_fetcher_->left_voltage(),
- output_fetcher_->right_voltage()});
- last_pico_timestamp_ = pico_timestamp;
- }
- {
- auto builder = status_sender_.MakeBuilder();
- const flatbuffers::Offset<ModelBasedStatus> model_based_status =
- model_based_.PopulateStatus(builder.fbb());
- const flatbuffers::Offset<control_loops::drivetrain::ImuZeroerState>
- zeroer_status = zeroer_.PopulateStatus(builder.fbb());
- LocalizerStatus::Builder status_builder =
- builder.MakeBuilder<LocalizerStatus>();
- status_builder.add_model_based(model_based_status);
- status_builder.add_zeroed(zeroer_.Zeroed());
- status_builder.add_faulted_zero(zeroer_.Faulted());
- status_builder.add_zeroing(zeroer_status);
- status_builder.add_left_encoder(encoders(0));
- status_builder.add_right_encoder(encoders(1));
- if (pico_offset_.has_value()) {
- status_builder.add_pico_offset_ns(pico_offset_.value().count());
- status_builder.add_pico_offset_error_ns(
- pico_offset_error_.count());
- }
- builder.CheckOk(builder.Send(status_builder.Finish()));
- }
- if (last_output_send_ + std::chrono::milliseconds(5) <
- event_loop_->context().monotonic_event_time) {
- auto builder = output_sender_.MakeBuilder();
- LocalizerOutput::Builder output_builder =
- builder.MakeBuilder<LocalizerOutput>();
- // TODO(james): Should we bother to try to estimate time offsets for
- // the pico?
- output_builder.add_monotonic_timestamp_ns(
- value->monotonic_timestamp_ns());
- output_builder.add_x(model_based_.xytheta()(0));
- output_builder.add_y(model_based_.xytheta()(1));
- output_builder.add_theta(model_based_.xytheta()(2));
- const Eigen::Quaterniond &orientation = model_based_.orientation();
- Quaternion quaternion;
- quaternion.mutate_x(orientation.x());
- quaternion.mutate_y(orientation.y());
- quaternion.mutate_z(orientation.z());
- quaternion.mutate_w(orientation.w());
- output_builder.add_orientation(&quaternion);
- builder.CheckOk(builder.Send(output_builder.Finish()));
- last_output_send_ = event_loop_->monotonic_now();
- }
- }
- });
-}
-
-} // namespace frc971::controls
diff --git a/y2022/control_loops/localizer/localizer.h b/y2022/control_loops/localizer/localizer.h
deleted file mode 100644
index d1eaf5c..0000000
--- a/y2022/control_loops/localizer/localizer.h
+++ /dev/null
@@ -1,248 +0,0 @@
-#ifndef Y2022_CONTROL_LOOPS_LOCALIZER_LOCALIZER_H_
-#define Y2022_CONTROL_LOOPS_LOCALIZER_LOCALIZER_H_
-
-#include "Eigen/Dense"
-#include "Eigen/Geometry"
-
-#include "aos/events/event_loop.h"
-#include "aos/containers/ring_buffer.h"
-#include "aos/time/time.h"
-#include "y2020/vision/sift/sift_generated.h"
-#include "y2022/control_loops/localizer/localizer_status_generated.h"
-#include "y2022/control_loops/localizer/localizer_output_generated.h"
-#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
-#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
-#include "frc971/control_loops/drivetrain/localizer_generated.h"
-#include "frc971/zeroing/imu_zeroer.h"
-#include "frc971/zeroing/wrap.h"
-
-namespace frc971::controls {
-
-namespace testing {
-class LocalizerTest;
-}
-
-// Localizer implementation that makes use of a 6-axis IMU, encoder readings,
-// drivetrain voltages, and camera returns to localize the robot. Meant to
-// be run on a raspberry pi.
-//
-// This operates on the principle that the drivetrain can be in one of two
-// modes:
-// 1) A "normal" mode where it is obeying the regular drivetrain model, with
-// minimal lateral motion and no major external disturbances. This is
-// referred to as the "model" mode in the code/variable names.
-// 2) An non-holonomic mode where the robot is just flying around on a 2-D
-// plane with no meaningful constraints (referred to as an "accel" model
-// in the code, because we rely primarily on the accelerometer readings).
-//
-// In order to determine which mode to be in, we attempt to track whether the
-// two models are diverging substantially. In order to do this, we maintain a
-// 1-second long queue of "branches". A branch is generated every X iterations
-// and contains a model state and an accel state. When the branch starts, the
-// two will have identical states. For the remaining 1 second, the model state
-// will evolve purely according to the drivetrian model, and the accel state
-// will evolve purely using IMU readings.
-//
-// When the branch reaches 1 second in age, we calculate a residual associated
-// with how much the accel and model based states diverged. If they have
-// diverged substantially, that implies that the model is a poor match for
-// whatever has been happening to the robot in the past second, so if we were
-// previously relying on the model, we will override the current "actual"
-// state with the branched accel state, and then continue to update the accel
-// state based on IMU readings.
-// If we are currently in the accel state, we will continue generating branches
-// until the branches stop diverging--this will indicate that the model
-// matches the accelerometer readings again, and so we will swap back to
-// the model-based state.
-//
-// TODO:
-// * Implement paying attention to camera readings.
-// * Tune for ADIS16505/real robot.
-class ModelBasedLocalizer {
- public:
- static constexpr size_t kX = 0;
- static constexpr size_t kY = 1;
- static constexpr size_t kTheta = 2;
-
- static constexpr size_t kVelocityX = 3;
- static constexpr size_t kVelocityY = 4;
- static constexpr size_t kNAccelStates = 5;
-
- static constexpr size_t kLeftEncoder = 3;
- static constexpr size_t kLeftVelocity = 4;
- static constexpr size_t kLeftVoltageError = 5;
- static constexpr size_t kRightEncoder = 6;
- static constexpr size_t kRightVelocity = 7;
- static constexpr size_t kRightVoltageError = 8;
- static constexpr size_t kNModelStates = 9;
-
- static constexpr size_t kNModelOutputs = 3;
-
- static constexpr size_t kNAccelOuputs = 1;
-
- static constexpr size_t kAccelX = 0;
- static constexpr size_t kAccelY = 1;
- static constexpr size_t kThetaRate = 2;
- static constexpr size_t kNAccelInputs = 3;
-
- static constexpr size_t kLeftVoltage = 0;
- static constexpr size_t kRightVoltage = 1;
- static constexpr size_t kNModelInputs = 2;
-
- // Branching period, in cycles.
- // Needs 10 to even stay alive, and still at ~96% CPU.
- // ~20 gives ~55-60% CPU.
- static constexpr int kBranchPeriod = 20;
-
- typedef Eigen::Matrix<double, kNModelStates, 1> ModelState;
- typedef Eigen::Matrix<double, kNAccelStates, 1> AccelState;
- typedef Eigen::Matrix<double, kNModelInputs, 1> ModelInput;
- typedef Eigen::Matrix<double, kNAccelInputs, 1> AccelInput;
-
- ModelBasedLocalizer(
- const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
- void HandleImu(aos::monotonic_clock::time_point t,
- const Eigen::Vector3d &gyro, const Eigen::Vector3d &accel,
- const Eigen::Vector2d encoders, const Eigen::Vector2d voltage);
- void HandleImageMatch(aos::monotonic_clock::time_point,
- const vision::sift::ImageMatchResult *) {
- LOG(FATAL) << "Unimplemented.";
- }
- void HandleReset(aos::monotonic_clock::time_point,
- const Eigen::Vector3d &xytheta);
-
- flatbuffers::Offset<ModelBasedStatus> PopulateStatus(
- flatbuffers::FlatBufferBuilder *fbb);
-
- Eigen::Vector3d xytheta() const {
- if (using_model_) {
- return current_state_.model_state.block<3, 1>(0, 0);
- } else {
- return current_state_.accel_state.block<3, 1>(0, 0);
- }
- }
-
- Eigen::Quaterniond orientation() const { return last_orientation_; }
-
- AccelState accel_state() const { return current_state_.accel_state; };
-
- void set_longitudinal_offset(double offset) { long_offset_ = offset; }
-
- private:
- struct CombinedState {
- AccelState accel_state;
- ModelState model_state;
- aos::monotonic_clock::time_point branch_time;
- double accumulated_divergence;
- };
-
- static flatbuffers::Offset<AccelBasedState> BuildAccelState(
- flatbuffers::FlatBufferBuilder *fbb, const AccelState &state);
-
- static flatbuffers::Offset<ModelBasedState> BuildModelState(
- flatbuffers::FlatBufferBuilder *fbb, const ModelState &state);
-
- Eigen::Matrix<double, kNModelStates, kNModelStates> AModel(
- const ModelState &state) const;
- Eigen::Matrix<double, kNAccelStates, kNAccelStates> AAccel() const;
- ModelState DiffModel(const ModelState &state, const ModelInput &U) const;
- AccelState DiffAccel(const AccelState &state, const AccelInput &U) const;
-
- ModelState UpdateModel(const ModelState &model, const ModelInput &input,
- aos::monotonic_clock::duration dt) const;
- AccelState UpdateAccel(const AccelState &accel, const AccelInput &input,
- aos::monotonic_clock::duration dt) const;
-
- AccelState AccelStateForModelState(const ModelState &state) const;
- ModelState ModelStateForAccelState(const AccelState &state,
- const Eigen::Vector2d &encoders,
- const double yaw_rate) const;
- double ModelDivergence(const CombinedState &state,
- const AccelInput &accel_inputs,
- const Eigen::Vector2d &filtered_accel,
- const ModelInput &model_inputs);
- void UpdateState(
- CombinedState *state,
- const Eigen::Matrix<double, kNModelStates, kNModelOutputs> &K,
- const Eigen::Matrix<double, kNModelOutputs, 1> &Z,
- const Eigen::Matrix<double, kNModelOutputs, kNModelStates> &H,
- const AccelInput &accel_input, const ModelInput &model_input,
- aos::monotonic_clock::duration dt);
-
- const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
- const StateFeedbackHybridPlantCoefficients<2, 2, 2, double>
- velocity_drivetrain_coefficients_;
- Eigen::Matrix<double, kNModelStates, kNModelStates> A_continuous_model_;
- Eigen::Matrix<double, kNAccelStates, kNAccelStates> A_continuous_accel_;
- Eigen::Matrix<double, kNModelStates, kNModelInputs> B_continuous_model_;
- Eigen::Matrix<double, kNAccelStates, kNAccelInputs> B_continuous_accel_;
-
- Eigen::Matrix<double, kNModelStates, kNModelStates> Q_continuous_model_;
-
- Eigen::Matrix<double, kNModelStates, kNModelStates> P_model_;
- // When we are following the model, we will, on each iteration:
- // 1) Perform a model-based update of a single state.
- // 2) Add a hypothetical non-model-based entry based on the current state.
- // 3) Evict too-old non-model-based entries.
- control_loops::drivetrain::DrivetrainUkf down_estimator_;
-
- // Buffer of old branches these are all created by initializing a new
- // model-based state based on the current state, and then initializing a new
- // accel-based state on top of that new model-based state (to eliminate the
- // impact of any lateral motion).
- // We then integrate up all of these states and observe how much the model and
- // accel based states of each branch compare to one another.
- aos::RingBuffer<CombinedState, 2000 / kBranchPeriod> branches_;
- int branch_counter_ = 0;
-
- CombinedState current_state_;
- aos::monotonic_clock::time_point t_ = aos::monotonic_clock::min_time;
- bool using_model_;
-
- // X position of the IMU, in meters. 0 = center of robot, positive = ahead of
- // center, negative = behind center.
- double long_offset_ = -0.15;
-
- double last_residual_ = 0.0;
- double filtered_residual_ = 0.0;
- Eigen::Vector2d filtered_residual_accel_ = Eigen::Vector2d::Zero();
- Eigen::Vector3d abs_accel_ = Eigen::Vector3d::Zero();
- double velocity_residual_ = 0.0;
- double accel_residual_ = 0.0;
- double theta_rate_residual_ = 0.0;
- int hysteresis_count_ = 0;
- Eigen::Quaterniond last_orientation_ = Eigen::Quaterniond::Identity();
-
- int clock_resets_ = 0;
-
- friend class testing::LocalizerTest;
-};
-
-class EventLoopLocalizer {
- public:
- EventLoopLocalizer(
- aos::EventLoop *event_loop,
- const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
-
- ModelBasedLocalizer *localizer() { return &model_based_; }
-
- private:
- aos::EventLoop *event_loop_;
- ModelBasedLocalizer model_based_;
- aos::Sender<LocalizerStatus> status_sender_;
- aos::Sender<LocalizerOutput> output_sender_;
- aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
- zeroing::ImuZeroer zeroer_;
- aos::monotonic_clock::time_point last_output_send_ =
- aos::monotonic_clock::min_time;
- std::optional<aos::monotonic_clock::time_point> last_pico_timestamp_;
- aos::monotonic_clock::duration pico_offset_error_;
- // t = pico_offset_ + pico_timestamp.
- // Note that this can drift over sufficiently long time periods!
- std::optional<std::chrono::nanoseconds> pico_offset_;
-
- zeroing::UnwrapSensor left_encoder_;
- zeroing::UnwrapSensor right_encoder_;
-};
-} // namespace frc971::controls
-#endif // Y2022_CONTROL_LOOPS_LOCALIZER_LOCALIZER_H_
diff --git a/y2022/control_loops/localizer/localizer_main.cc b/y2022/control_loops/localizer/localizer_main.cc
deleted file mode 100644
index 187a441..0000000
--- a/y2022/control_loops/localizer/localizer_main.cc
+++ /dev/null
@@ -1,21 +0,0 @@
-#include "aos/events/shm_event_loop.h"
-#include "aos/init.h"
-#include "y2022/control_loops/localizer/localizer.h"
-#include "y2022/control_loops/drivetrain/drivetrain_base.h"
-
-DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
-
-int main(int argc, char *argv[]) {
- aos::InitGoogle(&argc, &argv);
-
- aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
-
- aos::ShmEventLoop event_loop(&config.message());
- frc971::controls::EventLoopLocalizer localizer(
- &event_loop, ::y2022::control_loops::drivetrain::GetDrivetrainConfig());
-
- event_loop.Run();
-
- return 0;
-}
diff --git a/y2022/control_loops/localizer/localizer_output.fbs b/y2022/control_loops/localizer/localizer_output.fbs
deleted file mode 100644
index 03abf19..0000000
--- a/y2022/control_loops/localizer/localizer_output.fbs
+++ /dev/null
@@ -1,26 +0,0 @@
-namespace frc971.controls;
-
-// This provides a minimal output from the localizer that can be forwarded to
-// the roborio and used for corrections to its (simpler) localizer.
-
-struct Quaternion {
- w:double (id: 0);
- x:double (id: 1);
- y:double (id: 2);
- z:double (id: 3);
-}
-
-table LocalizerOutput {
- // Timestamp (on the source node) that this sample corresponds with. This
- // may be older than the sent time to account for delays in sensor readings.
- monotonic_timestamp_ns:int64 (id: 0);
- // Current x/y position estimate, in meters.
- x:double (id: 1);
- y:double (id: 2);
- // Current heading, in radians.
- theta:double (id: 3);
- // Current estimate of the robot's 3-D rotation.
- orientation:Quaternion (id: 4);
-}
-
-root_type LocalizerOutput;
diff --git a/y2022/control_loops/localizer/localizer_plotter.ts b/y2022/control_loops/localizer/localizer_plotter.ts
deleted file mode 100644
index 239d2cb..0000000
--- a/y2022/control_loops/localizer/localizer_plotter.ts
+++ /dev/null
@@ -1,277 +0,0 @@
-// Provides a plot for debugging drivetrain-related issues.
-import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
-import {ImuMessageHandler} from 'org_frc971/frc971/wpilib/imu_plot_utils';
-import * as proxy from 'org_frc971/aos/network/www/proxy';
-import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
-
-import Connection = proxy.Connection;
-
-const TIME = AosPlotter.TIME;
-const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
-const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
-
-export function plotLocalizer(conn: Connection, element: Element): void {
- const aosPlotter = new AosPlotter(conn);
-
- const position = aosPlotter.addMessageSource("/drivetrain",
- "frc971.control_loops.drivetrain.Position");
- const status = aosPlotter.addMessageSource(
- '/drivetrain', 'frc971.control_loops.drivetrain.Status');
- const output = aosPlotter.addMessageSource(
- '/drivetrain', 'frc971.control_loops.drivetrain.Output');
- const localizer = aosPlotter.addMessageSource(
- '/localizer', 'frc971.controls.LocalizerStatus');
- const imu = aosPlotter.addRawMessageSource(
- '/localizer', 'frc971.IMUValuesBatch',
- new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
-
- // Drivetrain Status estimated relative position
- const positionPlot = aosPlotter.addPlot(element);
- positionPlot.plot.getAxisLabels().setTitle("Estimated Relative Position " +
- "of the Drivetrain");
- positionPlot.plot.getAxisLabels().setXLabel(TIME);
- positionPlot.plot.getAxisLabels().setYLabel("Relative Position (m)");
- const leftPosition =
- positionPlot.addMessageLine(status, ["estimated_left_position"]);
- leftPosition.setColor(RED);
- const rightPosition =
- positionPlot.addMessageLine(status, ["estimated_right_position"]);
- rightPosition.setColor(GREEN);
- positionPlot
- .addMessageLine(localizer, ['model_based', 'model_state', 'left_encoder'])
- .setColor(BROWN)
- .setPointSize(0.0);
- positionPlot
- .addMessageLine(localizer, ['model_based', 'model_state', 'right_encoder'])
- .setColor(CYAN)
- .setPointSize(0.0);
- positionPlot.addMessageLine(position, ['left_encoder'])
- .setColor(BROWN)
- .setDrawLine(false);
- positionPlot.addMessageLine(imu, ['left_encoder'])
- .setColor(BROWN)
- .setDrawLine(false);
- positionPlot.addMessageLine(localizer, ['left_encoder'])
- .setColor(RED)
- .setDrawLine(false);
- positionPlot.addMessageLine(position, ['right_encoder'])
- .setColor(CYAN)
- .setDrawLine(false);
- positionPlot.addMessageLine(imu, ['right_encoder'])
- .setColor(CYAN)
- .setDrawLine(false);
- positionPlot.addMessageLine(localizer, ['right_encoder'])
- .setColor(GREEN)
- .setDrawLine(false);
-
-
- // Drivetrain Velocities
- const velocityPlot = aosPlotter.addPlot(element);
- velocityPlot.plot.getAxisLabels().setTitle('Velocity Plots');
- velocityPlot.plot.getAxisLabels().setXLabel(TIME);
- velocityPlot.plot.getAxisLabels().setYLabel('Wheel Velocity (m/s)');
-
- const leftVelocity =
- velocityPlot.addMessageLine(status, ['estimated_left_velocity']);
- leftVelocity.setColor(RED);
- const rightVelocity =
- velocityPlot.addMessageLine(status, ['estimated_right_velocity']);
- rightVelocity.setColor(GREEN);
-
- const leftSpeed = velocityPlot.addMessageLine(position, ["left_speed"]);
- leftSpeed.setColor(BLUE);
- const rightSpeed = velocityPlot.addMessageLine(position, ["right_speed"]);
- rightSpeed.setColor(BROWN);
-
- const ekfLeftVelocity = velocityPlot.addMessageLine(
- localizer, ['model_based', 'model_state', 'left_velocity']);
- ekfLeftVelocity.setColor(RED);
- ekfLeftVelocity.setPointSize(0.0);
- const ekfRightVelocity = velocityPlot.addMessageLine(
- localizer, ['model_based', 'model_state', 'right_velocity']);
- ekfRightVelocity.setColor(GREEN);
- ekfRightVelocity.setPointSize(0.0);
-
- velocityPlot
- .addMessageLine(
- localizer, ['model_based', 'oldest_model_state', 'left_velocity'])
- .setColor(RED)
- .setDrawLine(false);
-
- velocityPlot
- .addMessageLine(
- localizer, ['model_based', 'oldest_model_state', 'right_velocity'])
- .setColor(GREEN)
- .setDrawLine(false);
-
- const splineVelocityOffset =
- velocityPlot
- .addMessageLine(status, ['localizer', 'longitudinal_velocity_offset'])
- .setColor(BROWN)
- .setPointSize(0.0);
-
- const splineLateralVelocity =
- velocityPlot.addMessageLine(status, ['localizer', 'lateral_velocity'])
- .setColor(PINK)
- .setPointSize(0.0);
-
- // Drivetrain Voltage
- const voltagePlot = aosPlotter.addPlot(element);
- voltagePlot.plot.getAxisLabels().setTitle('Voltage Plots');
- voltagePlot.plot.getAxisLabels().setXLabel(TIME);
- voltagePlot.plot.getAxisLabels().setYLabel('Voltage (V)')
-
- voltagePlot.addMessageLine(localizer, ['model_based', 'model_state', 'left_voltage_error'])
- .setColor(RED)
- .setDrawLine(false);
- voltagePlot.addMessageLine(localizer, ['model_based', 'model_state', 'right_voltage_error'])
- .setColor(GREEN)
- .setDrawLine(false);
- voltagePlot.addMessageLine(output, ['left_voltage'])
- .setColor(RED)
- .setPointSize(0);
- voltagePlot.addMessageLine(output, ['right_voltage'])
- .setColor(GREEN)
- .setPointSize(0);
-
- // Heading
- const yawPlot = aosPlotter.addPlot(element);
- yawPlot.plot.getAxisLabels().setTitle('Robot Yaw');
- yawPlot.plot.getAxisLabels().setXLabel(TIME);
- yawPlot.plot.getAxisLabels().setYLabel('Yaw (rad)');
-
- yawPlot.addMessageLine(status, ['localizer', 'theta']).setColor(GREEN);
-
- yawPlot.addMessageLine(status, ['down_estimator', 'yaw']).setColor(BLUE);
-
- yawPlot.addMessageLine(localizer, ['model_based', 'theta']).setColor(RED);
-
- // Pitch/Roll
- const orientationPlot = aosPlotter.addPlot(element);
- orientationPlot.plot.getAxisLabels().setTitle('Orientation');
- orientationPlot.plot.getAxisLabels().setXLabel(TIME);
- orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
-
- orientationPlot.addMessageLine(localizer, ['model_based', 'down_estimator', 'lateral_pitch'])
- .setColor(RED)
- .setLabel('roll');
- orientationPlot
- .addMessageLine(localizer, ['model_based', 'down_estimator', 'longitudinal_pitch'])
- .setColor(GREEN)
- .setLabel('pitch');
-
- const stillPlot = aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 3]);
- stillPlot.plot.getAxisLabels().setTitle('Still Plot');
- stillPlot.plot.getAxisLabels().setXLabel(TIME);
- stillPlot.plot.getAxisLabels().setYLabel('bool, g\'s');
- stillPlot.plot.setDefaultYRange([-0.1, 1.1]);
-
- stillPlot.addMessageLine(localizer, ['model_based', 'down_estimator', 'gravity_magnitude'])
- .setColor(WHITE)
- .setDrawLine(false);
- stillPlot.addMessageLine(localizer, ['model_based', 'using_model'])
- .setColor(PINK)
- .setDrawLine(false);
-
- // Accelerometer/Gravity
- const accelPlot = aosPlotter.addPlot(element);
- accelPlot.plot.getAxisLabels().setTitle('Absoluate Velocities')
- accelPlot.plot.getAxisLabels().setYLabel('Velocity (m/s)');
- accelPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
-
- accelPlot.addMessageLine(localizer, ['model_based', 'accel_state', 'velocity_x'])
- .setColor(RED)
- .setDrawLine(false);
- accelPlot.addMessageLine(localizer, ['model_based', 'accel_state', 'velocity_y'])
- .setColor(GREEN)
- .setDrawLine(false);
-
- accelPlot.addMessageLine(localizer, ['model_based', 'oldest_accel_state', 'velocity_x'])
- .setColor(RED)
- .setPointSize(0);
- accelPlot.addMessageLine(localizer, ['model_based', 'oldest_accel_state', 'velocity_y'])
- .setColor(GREEN)
- .setPointSize(0);
-
- // Absolute X Position
- const xPositionPlot = aosPlotter.addPlot(element);
- xPositionPlot.plot.getAxisLabels().setTitle('X Position');
- xPositionPlot.plot.getAxisLabels().setXLabel(TIME);
- xPositionPlot.plot.getAxisLabels().setYLabel('X Position (m)');
-
- xPositionPlot.addMessageLine(status, ['x']).setColor(RED);
- xPositionPlot.addMessageLine(status, ['down_estimator', 'position_x'])
- .setColor(BLUE);
- xPositionPlot.addMessageLine(localizer, ['model_based', 'x']).setColor(CYAN);
-
- // Absolute Y Position
- const yPositionPlot = aosPlotter.addPlot(element);
- yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
- yPositionPlot.plot.getAxisLabels().setXLabel(TIME);
- yPositionPlot.plot.getAxisLabels().setYLabel('Y Position (m)');
-
- const localizerY = yPositionPlot.addMessageLine(status, ['y']);
- localizerY.setColor(RED);
- yPositionPlot.addMessageLine(status, ['down_estimator', 'position_y'])
- .setColor(BLUE);
- yPositionPlot.addMessageLine(localizer, ['model_based', 'y']).setColor(CYAN);
-
- // Gyro
- const gyroPlot = aosPlotter.addPlot(element);
- gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
- gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
- gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
-
- const gyroX = gyroPlot.addMessageLine(imu, ['gyro_x']);
- gyroX.setColor(RED);
- const gyroY = gyroPlot.addMessageLine(imu, ['gyro_y']);
- gyroY.setColor(GREEN);
- const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
- gyroZ.setColor(BLUE);
-
- const impliedAccelPlot =
- aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- impliedAccelPlot.plot.getAxisLabels().setTitle('Implied Accelerations');
- impliedAccelPlot.plot.getAxisLabels().setXLabel(TIME);
-
- impliedAccelPlot.addMessageLine(localizer, ['model_based', 'implied_accel_z'])
- .setColor(BLUE);
- impliedAccelPlot.addMessageLine(localizer, ['model_based', 'implied_accel_y'])
- .setColor(GREEN);
- impliedAccelPlot.addMessageLine(localizer, ['model_based', 'implied_accel_x'])
- .setColor(RED);
-
- const costPlot =
- aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- costPlot.plot.getAxisLabels().setTitle('Costs');
- costPlot.plot.getAxisLabels().setXLabel(TIME);
-
- costPlot.addMessageLine(localizer, ['model_based', 'residual'])
- .setColor(RED)
- .setPointSize(0);
-
- costPlot.addMessageLine(localizer, ['model_based', 'filtered_residual'])
- .setColor(BLUE)
- .setPointSize(0);
-
- costPlot.addMessageLine(localizer, ['model_based', 'velocity_residual'])
- .setColor(GREEN)
- .setPointSize(0);
-
- costPlot.addMessageLine(localizer, ['model_based', 'theta_rate_residual'])
- .setColor(BROWN)
- .setPointSize(0);
-
- costPlot.addMessageLine(localizer, ['model_based', 'accel_residual'])
- .setColor(CYAN)
- .setPointSize(0);
-
- const timingPlot =
- aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
- timingPlot.plot.getAxisLabels().setTitle('Timing');
- timingPlot.plot.getAxisLabels().setXLabel(TIME);
-
- timingPlot.addMessageLine(localizer, ['model_based', 'clock_resets'])
- .setColor(GREEN)
- .setDrawLine(false);
-}
diff --git a/y2022/control_loops/localizer/localizer_replay.cc b/y2022/control_loops/localizer/localizer_replay.cc
deleted file mode 100644
index 199882c..0000000
--- a/y2022/control_loops/localizer/localizer_replay.cc
+++ /dev/null
@@ -1,96 +0,0 @@
-
-#include "aos/configuration.h"
-#include "aos/events/logging/log_reader.h"
-#include "aos/events/logging/log_writer.h"
-#include "aos/events/simulated_event_loop.h"
-#include "aos/init.h"
-#include "aos/json_to_flatbuffer.h"
-#include "aos/network/team_number.h"
-#include "y2022/control_loops/localizer/localizer.h"
-#include "y2022/control_loops/localizer/localizer_schema.h"
-#include "gflags/gflags.h"
-#include "y2022/control_loops/drivetrain/drivetrain_base.h"
-
-DEFINE_string(config, "y2022/aos_config.json",
- "Name of the config file to replay using.");
-DEFINE_int32(team, 7971, "Team number to use for logfile replay.");
-DEFINE_string(output_folder, "/tmp/replayed",
- "Name of the folder to write replayed logs to.");
-
-class LoggerState {
- public:
- LoggerState(aos::logger::LogReader *reader, const aos::Node *node)
- : event_loop_(
- reader->event_loop_factory()->MakeEventLoop("logger", node)),
- namer_(std::make_unique<aos::logger::MultiNodeLogNamer>(
- absl::StrCat(FLAGS_output_folder, "/", node->name()->string_view(),
- "/"),
- event_loop_.get())),
- logger_(std::make_unique<aos::logger::Logger>(event_loop_.get())) {
- event_loop_->SkipTimingReport();
- event_loop_->SkipAosLog();
- event_loop_->OnRun([this]() { logger_->StartLogging(std::move(namer_)); });
- }
-
- private:
- std::unique_ptr<aos::EventLoop> event_loop_;
- std::unique_ptr<aos::logger::LogNamer> namer_;
- std::unique_ptr<aos::logger::Logger> logger_;
-};
-
-// TODO(james): Currently, this replay produces logfiles that can't be read due
-// to time estimation issues. Pending the active refactorings of the
-// timestamp-related code, fix this.
-int main(int argc, char **argv) {
- aos::InitGoogle(&argc, &argv);
-
- aos::network::OverrideTeamNumber(FLAGS_team);
-
- const aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
-
- // find logfiles
- std::vector<std::string> unsorted_logfiles =
- aos::logger::FindLogs(argc, argv);
-
- // sort logfiles
- const std::vector<aos::logger::LogFile> logfiles =
- aos::logger::SortParts(unsorted_logfiles);
-
- // open logfiles
- aos::logger::LogReader reader(logfiles, &config.message());
-
- auto factory =
- std::make_unique<aos::SimulatedEventLoopFactory>(reader.configuration());
-
- reader.Register(factory.get());
-
- std::vector<std::unique_ptr<LoggerState>> loggers;
- // List of nodes to create loggers for (note: currently just roborio; this
- // code was refactored to allow easily adding new loggers to accommodate
- // debugging and potential future changes).
- const std::vector<std::string> nodes_to_log = {"imu"};
- for (const std::string &node : nodes_to_log) {
- loggers.emplace_back(std::make_unique<LoggerState>(
- &reader, aos::configuration::GetNode(reader.configuration(), node)));
- }
-
- const aos::Node *node = nullptr;
- if (aos::configuration::MultiNode(reader.configuration())) {
- node = aos::configuration::GetNode(reader.configuration(), "imu");
- }
-
- std::unique_ptr<aos::EventLoop> localizer_event_loop =
- reader.event_loop_factory()->MakeEventLoop("localizer", node);
- localizer_event_loop->SkipTimingReport();
-
- frc971::controls::EventLoopLocalizer localizer(
- localizer_event_loop.get(),
- y2022::control_loops::drivetrain::GetDrivetrainConfig());
-
- reader.event_loop_factory()->Run();
-
- reader.Deregister();
-
- return 0;
-}
diff --git a/y2022/control_loops/localizer/localizer_status.fbs b/y2022/control_loops/localizer/localizer_status.fbs
deleted file mode 100644
index 80cee0b..0000000
--- a/y2022/control_loops/localizer/localizer_status.fbs
+++ /dev/null
@@ -1,99 +0,0 @@
-include "frc971/control_loops/drivetrain/drivetrain_status.fbs";
-
-namespace frc971.controls;
-
-// Stores the state associated with the acceleration-based modelling.
-table AccelBasedState {
- // x/y position, in meters.
- x:double (id: 0);
- y:double (id: 1);
- // heading, in radians.
- theta:double (id: 2);
- // Velocity in X/Y directions, in m/s.
- velocity_x:double (id: 3);
- velocity_y:double (id: 4);
-}
-
-// Stores the state associated with the drivetrain model-based state.
-// This model assumes zero lateral motion of the drivetrain.
-table ModelBasedState {
- // x/y position, in meters.
- x:double (id: 0);
- y:double (id: 1);
- // heading, in radians.
- theta:double (id: 2);
- // Expected encoder reading for the left side of the drivetrain, in meters.
- left_encoder:double (id: 3);
- // Modelled velocity of the left side of the drivetrain, in meters / second.
- left_velocity:double (id: 4);
- // Estimated voltage error, in volts.
- left_voltage_error:double (id: 5);
- // Same as the left_* fields, but for the right side of the drivetrain.
- right_encoder:double (id: 6);
- right_velocity:double (id: 7);
- right_voltage_error:double (id: 8);
-}
-
-table ModelBasedStatus {
- // Current acceleration and model-based states. Depending on using_model,
- // one of these will be the ground-truth and the other will be calculated
- // based on it. E.g. if using_model is true, then model_state will be
- // populated as you'd expect, while accel_state will be populated to be
- // consistent with model_state (e.g., no lateral motion).
- accel_state:AccelBasedState (id: 0);
- model_state:ModelBasedState (id: 1);
- // using_model indicates whether we are currently in in model-based or
- // accelerometer-based estimation.
- using_model:bool (id: 2);
- // Current residual associated with the amount of inconsistency between
- // the two models. Will be zero if the drivetrain model is perfectly
- // consistent with the IMU readings.
- residual:double (id: 3);
- // Status from the down estimator.
- down_estimator:frc971.control_loops.drivetrain.DownEstimatorState (id: 4);
- // Current ground-truth for x/y/theta. Should match those present in *_state.
- x:double (id: 5);
- y:double (id: 6);
- theta:double (id: 7);
- // Current accelerations implied by the current accelerometer + down estimator
- // + yaw readings.
- implied_accel_x:double (id: 8);
- implied_accel_y:double (id: 9);
- implied_accel_z:double (id: 10);
- // oldest_* are the oldest surviving branches of the model that have just been
- // running purely on one model.
- oldest_accel_state:AccelBasedState (id: 11);
- oldest_model_state:ModelBasedState (id: 12);
- // Filtered version of the residual field--this is what is actually used by
- // the code for determining when to swap between modes.
- filtered_residual:double (id: 13);
- // Components of the residual. Useful for debugging.
- velocity_residual:double (id: 14);
- accel_residual:double (id: 15);
- theta_rate_residual:double (id: 16);
- // Number of times we have missed an IMU reading. Should never increase except
- // *maybe* during startup.
- clock_resets:int (id: 17);
-}
-
-table LocalizerStatus {
- model_based:ModelBasedStatus (id: 0);
- // Whether the IMU is zeroed or not.
- zeroed:bool (id: 1);
- // Whether the IMU zeroing is faulted or not.
- faulted_zero:bool (id: 2);
- zeroing:control_loops.drivetrain.ImuZeroerState (id: 3);
- // Offset between the pico clock and the pi clock, such that
- // pico_timestamp + pico_offset_ns = pi_timestamp
- pico_offset_ns:int64 (id: 4);
- // Error in the offset, if we assume that the pi/pico clocks are identical and
- // that there is a perfectly consistent latency between the two. Will be zero
- // for the very first cycle, and then referenced off of the initial offset
- // thereafter. If greater than zero, implies that the pico is "behind",
- // whether due to unusually large latency or due to clock drift.
- pico_offset_error_ns:int64 (id: 5);
- left_encoder:double (id: 6);
- right_encoder:double (id: 7);
-}
-
-root_type LocalizerStatus;
diff --git a/y2022/control_loops/localizer/localizer_test.cc b/y2022/control_loops/localizer/localizer_test.cc
deleted file mode 100644
index c58f664..0000000
--- a/y2022/control_loops/localizer/localizer_test.cc
+++ /dev/null
@@ -1,528 +0,0 @@
-#include "y2022/control_loops/localizer/localizer.h"
-
-#include "aos/events/simulated_event_loop.h"
-#include "gtest/gtest.h"
-#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
-
-namespace frc971::controls::testing {
-typedef ModelBasedLocalizer::ModelState ModelState;
-typedef ModelBasedLocalizer::AccelState AccelState;
-typedef ModelBasedLocalizer::ModelInput ModelInput;
-typedef ModelBasedLocalizer::AccelInput AccelInput;
-namespace {
-constexpr size_t kX = ModelBasedLocalizer::kX;
-constexpr size_t kY = ModelBasedLocalizer::kY;
-constexpr size_t kTheta = ModelBasedLocalizer::kTheta;
-constexpr size_t kVelocityX = ModelBasedLocalizer::kVelocityX;
-constexpr size_t kVelocityY = ModelBasedLocalizer::kVelocityY;
-constexpr size_t kAccelX = ModelBasedLocalizer::kAccelX;
-constexpr size_t kAccelY = ModelBasedLocalizer::kAccelY;
-constexpr size_t kThetaRate = ModelBasedLocalizer::kThetaRate;
-constexpr size_t kLeftEncoder = ModelBasedLocalizer::kLeftEncoder;
-constexpr size_t kLeftVelocity = ModelBasedLocalizer::kLeftVelocity;
-constexpr size_t kLeftVoltageError = ModelBasedLocalizer::kLeftVoltageError;
-constexpr size_t kRightEncoder = ModelBasedLocalizer::kRightEncoder;
-constexpr size_t kRightVelocity = ModelBasedLocalizer::kRightVelocity;
-constexpr size_t kRightVoltageError = ModelBasedLocalizer::kRightVoltageError;
-constexpr size_t kLeftVoltage = ModelBasedLocalizer::kLeftVoltage;
-constexpr size_t kRightVoltage = ModelBasedLocalizer::kRightVoltage;
-}
-
-class LocalizerTest : public ::testing::Test {
- protected:
- LocalizerTest()
- : dt_config_(
- control_loops::drivetrain::testing::GetTestDrivetrainConfig()),
- localizer_(dt_config_) {
- localizer_.set_longitudinal_offset(0.0);
- }
- ModelState CallDiffModel(const ModelState &state, const ModelInput &U) {
- return localizer_.DiffModel(state, U);
- }
-
- AccelState CallDiffAccel(const AccelState &state, const AccelInput &U) {
- return localizer_.DiffAccel(state, U);
- }
-
- const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
- ModelBasedLocalizer localizer_;
-
-};
-
-TEST_F(LocalizerTest, AccelIntegrationTest) {
- AccelState state;
- state.setZero();
- AccelInput input;
- input.setZero();
-
- EXPECT_EQ(0.0, CallDiffAccel(state, input).norm());
- // Non-zero x/y/theta states should still result in a zero derivative.
- state(kX) = 1.0;
- state(kY) = 1.0;
- state(kTheta) = 1.0;
- EXPECT_EQ(0.0, CallDiffAccel(state, input).norm());
-
- state.setZero();
- state(kVelocityX) = 1.0;
- state(kVelocityY) = 2.0;
- EXPECT_EQ((AccelState() << 1.0, 2.0, 0.0, 0.0, 0.0).finished(),
- CallDiffAccel(state, input));
- // Derivatives should be independent of theta.
- state(kTheta) = M_PI / 2.0;
- EXPECT_EQ((AccelState() << 1.0, 2.0, 0.0, 0.0, 0.0).finished(),
- CallDiffAccel(state, input));
-
- state.setZero();
- input(kAccelX) = 1.0;
- input(kAccelY) = 2.0;
- input(kThetaRate) = 3.0;
- EXPECT_EQ((AccelState() << 0.0, 0.0, 3.0, 1.0, 2.0).finished(),
- CallDiffAccel(state, input));
- state(kTheta) = M_PI / 2.0;
- EXPECT_EQ((AccelState() << 0.0, 0.0, 3.0, 1.0, 2.0).finished(),
- CallDiffAccel(state, input));
-}
-
-TEST_F(LocalizerTest, ModelIntegrationTest) {
- ModelState state;
- state.setZero();
- ModelInput input;
- input.setZero();
- ModelState diff;
-
- EXPECT_EQ(0.0, CallDiffModel(state, input).norm());
- // Non-zero x/y/theta/encoder states should still result in a zero derivative.
- state(kX) = 1.0;
- state(kY) = 1.0;
- state(kTheta) = 1.0;
- state(kLeftEncoder) = 1.0;
- state(kRightEncoder) = 1.0;
- EXPECT_EQ(0.0, CallDiffModel(state, input).norm());
-
- state.setZero();
- state(kLeftVelocity) = 1.0;
- state(kRightVelocity) = 1.0;
- diff = CallDiffModel(state, input);
- const ModelState mask_velocities =
- (ModelState() << 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0).finished();
- EXPECT_EQ(
- (ModelState() << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(),
- diff.cwiseProduct(mask_velocities));
- EXPECT_EQ(diff(kLeftVelocity), diff(kRightVelocity));
- EXPECT_GT(0.0, diff(kLeftVelocity));
- state(kTheta) = M_PI / 2.0;
- diff = CallDiffModel(state, input);
- EXPECT_NEAR(0.0,
- ((ModelState() << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0)
- .finished() -
- diff.cwiseProduct(mask_velocities))
- .norm(),
- 1e-12);
- EXPECT_EQ(diff(kLeftVelocity), diff(kRightVelocity));
- EXPECT_GT(0.0, diff(kLeftVelocity));
-
- state.setZero();
- state(kLeftVelocity) = -1.0;
- state(kRightVelocity) = 1.0;
- diff = CallDiffModel(state, input);
- EXPECT_EQ((ModelState() << 0.0, 0.0, 1.0 / dt_config_.robot_radius, -1.0, 0.0,
- 0.0, 1.0, 0.0, 0.0)
- .finished(),
- diff.cwiseProduct(mask_velocities));
- EXPECT_EQ(-diff(kLeftVelocity), diff(kRightVelocity));
- EXPECT_LT(0.0, diff(kLeftVelocity));
-
- state.setZero();
- input(kLeftVoltage) = 5.0;
- input(kRightVoltage) = 6.0;
- diff = CallDiffModel(state, input);
- EXPECT_EQ(0, diff(kX));
- EXPECT_EQ(0, diff(kY));
- EXPECT_EQ(0, diff(kTheta));
- EXPECT_EQ(0, diff(kLeftEncoder));
- EXPECT_EQ(0, diff(kRightEncoder));
- EXPECT_EQ(0, diff(kLeftVoltageError));
- EXPECT_EQ(0, diff(kRightVoltageError));
- EXPECT_LT(0, diff(kLeftVelocity));
- EXPECT_LT(0, diff(kRightVelocity));
- EXPECT_LT(diff(kLeftVelocity), diff(kRightVelocity));
-
- state.setZero();
- state(kLeftVoltageError) = -1.0;
- state(kRightVoltageError) = -2.0;
- input(kLeftVoltage) = 1.0;
- input(kRightVoltage) = 2.0;
- EXPECT_EQ(ModelState::Zero(), CallDiffModel(state, input));
-}
-
-// Test that the HandleReset does indeed reset the state of the localizer.
-TEST_F(LocalizerTest, LocalizerReset) {
- aos::monotonic_clock::time_point t = aos::monotonic_clock::epoch();
- localizer_.HandleReset(t, {1.0, 2.0, 3.0});
- EXPECT_EQ((Eigen::Vector3d{1.0, 2.0, 3.0}), localizer_.xytheta());
- localizer_.HandleReset(t, {4.0, 5.0, 6.0});
- EXPECT_EQ((Eigen::Vector3d{4.0, 5.0, 6.0}), localizer_.xytheta());
-}
-
-// Test that if we are moving only by accelerometer readings (and just assuming
-// zero voltage/encoders) that we initially don't believe it but then latch into
-// following the accelerometer.
-// Note: this test is somewhat sensitive to the exact tuning values used for the
-// filter.
-TEST_F(LocalizerTest, AccelOnly) {
- const aos::monotonic_clock::time_point start = aos::monotonic_clock::epoch();
- const std::chrono::microseconds kDt{500};
- aos::monotonic_clock::time_point t = start - std::chrono::milliseconds(1000);
- Eigen::Vector3d gyro{0.0, 0.0, 0.0};
- const Eigen::Vector2d encoders{0.0, 0.0};
- const Eigen::Vector2d voltages{0.0, 0.0};
- Eigen::Vector3d accel{5.0, 2.0, 9.80665};
- Eigen::Vector3d accel_gs = accel / 9.80665;
- while (t < start) {
- // Spin to fill up the buffer.
- localizer_.HandleImu(t, gyro, Eigen::Vector3d::UnitZ(), encoders, voltages);
- t += kDt;
- }
- while (t < start + std::chrono::milliseconds(100)) {
- localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
- EXPECT_EQ(Eigen::Vector3d::Zero(), localizer_.xytheta());
- t += kDt;
- }
- while (t < start + std::chrono::milliseconds(500)) {
- // Too lazy to hard-code when the transition happens.
- localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
- t += kDt;
- }
- while (t < start + std::chrono::milliseconds(1000)) {
- SCOPED_TRACE(t);
- localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
- const Eigen::Vector3d xytheta = localizer_.xytheta();
- t += kDt;
- EXPECT_NEAR(
- 0.5 * accel(0) * std::pow(aos::time::DurationInSeconds(t - start), 2),
- xytheta(0), 1e-4);
- EXPECT_NEAR(
- 0.5 * accel(1) * std::pow(aos::time::DurationInSeconds(t - start), 2),
- xytheta(1), 1e-4);
- EXPECT_EQ(0.0, xytheta(2));
- }
-
- ASSERT_NEAR(accel(0), localizer_.accel_state()(kVelocityX), 1e-10);
- ASSERT_NEAR(accel(1), localizer_.accel_state()(kVelocityY), 1e-10);
-
- // Start going in a cirlce, and confirm that we
- // handle things correctly. We rotate the accelerometer readings by 90 degrees
- // and then leave them constant, which should make it look like we are going
- // around in a circle.
- accel = Eigen::Vector3d{-accel(1), accel(0), 9.80665};
- accel_gs = accel / 9.80665;
- // v^2 / r = a
- // w * r = v
- // v^2 / v * w = a
- // w = a / v
- const double omega = accel.topRows<2>().norm() /
- std::hypot(localizer_.accel_state()(kVelocityX),
- localizer_.accel_state()(kVelocityY));
- gyro << 0.0, 0.0, omega;
- // Due to the magic of math, omega works out to be 1.0 after having run at the
- // acceleration for one second.
- ASSERT_NEAR(1.0, omega, 1e-10);
- // Yes, we could save some operations here, but let's be at least somewhat
- // clear about what we're doing...
- const double radius = accel.topRows<2>().norm() / (omega * omega);
- const Eigen::Vector2d center = localizer_.xytheta().topRows<2>() +
- accel.topRows<2>().normalized() * radius;
- const double initial_theta = std::atan2(-accel(1), -accel(0));
-
- std::chrono::microseconds one_revolution_time(
- static_cast<int>(2 * M_PI / omega * 1e6));
-
- aos::monotonic_clock::time_point circle_start = t;
-
- while (t < circle_start + one_revolution_time) {
- SCOPED_TRACE(t);
- localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
- t += kDt;
- const double t_circle = aos::time::DurationInSeconds(t - circle_start);
- ASSERT_NEAR(t_circle * omega, localizer_.xytheta()(2), 1e-5);
- const double theta_circle = t_circle * omega + initial_theta;
- const Eigen::Vector2d offset =
- radius *
- Eigen::Vector2d{std::cos(theta_circle), std::sin(theta_circle)};
- const Eigen::Vector2d expected = center + offset;
- const Eigen::Vector2d estimated = localizer_.xytheta().topRows<2>();
- const Eigen::Vector2d implied_offset = estimated - center;
- const double implied_theta =
- std::atan2(implied_offset.y(), implied_offset.x());
- VLOG(1) << "center: " << center.transpose() << " radius " << radius
- << "\nlocalizer " << localizer_.xytheta().transpose()
- << " t_circle " << t_circle << " omega " << omega << " theta "
- << theta_circle << "\noffset " << offset.transpose()
- << "\nexpected " << expected.transpose() << "\nimplied offset "
- << implied_offset << " implied_theta " << implied_theta << "\nvel "
- << localizer_.accel_state()(kVelocityX) << ", "
- << localizer_.accel_state()(kVelocityY);
- ASSERT_NEAR(0.0, (expected - localizer_.xytheta().topRows<2>()).norm(),
- 1e-2);
- }
-
- // Set accelerometer back to zero and confirm that we recover (the
- // implementation decays the accelerometer speeds to zero when still, so
- // should recover).
- while (t <
- circle_start + one_revolution_time + std::chrono::milliseconds(3000)) {
- localizer_.HandleImu(t, Eigen::Vector3d::Zero(), Eigen::Vector3d::UnitZ(),
- encoders, voltages);
- t += kDt;
- }
- const Eigen::Vector3d final_pos = localizer_.xytheta();
- localizer_.HandleImu(t, Eigen::Vector3d::Zero(), Eigen::Vector3d::UnitZ(),
- encoders, voltages);
- ASSERT_NEAR(0.0, (final_pos - localizer_.xytheta()).norm(), 1e-10);
-}
-
-using control_loops::drivetrain::Output;
-
-class EventLoopLocalizerTest : public ::testing::Test {
- protected:
- EventLoopLocalizerTest()
- : configuration_(aos::configuration::ReadConfig("y2022/aos_config.json")),
- event_loop_factory_(&configuration_.message()),
- roborio_node_(
- aos::configuration::GetNode(&configuration_.message(), "roborio")),
- imu_node_(
- aos::configuration::GetNode(&configuration_.message(), "imu")),
- dt_config_(
- control_loops::drivetrain::testing::GetTestDrivetrainConfig()),
- localizer_event_loop_(
- event_loop_factory_.MakeEventLoop("localizer", imu_node_)),
- localizer_(localizer_event_loop_.get(), dt_config_),
- drivetrain_plant_event_loop_(event_loop_factory_.MakeEventLoop(
- "drivetrain_plant", roborio_node_)),
- drivetrain_plant_imu_event_loop_(
- event_loop_factory_.MakeEventLoop("drivetrain_plant", imu_node_)),
- drivetrain_plant_(drivetrain_plant_event_loop_.get(),
- drivetrain_plant_imu_event_loop_.get(), dt_config_,
- std::chrono::microseconds(500)),
- roborio_test_event_loop_(
- event_loop_factory_.MakeEventLoop("test", roborio_node_)),
- imu_test_event_loop_(
- event_loop_factory_.MakeEventLoop("test", imu_node_)),
- logger_test_event_loop_(
- event_loop_factory_.GetNodeEventLoopFactory("logger")
- ->MakeEventLoop("test")),
- output_sender_(
- roborio_test_event_loop_->MakeSender<Output>("/drivetrain")),
- output_fetcher_(roborio_test_event_loop_->MakeFetcher<LocalizerOutput>(
- "/localizer")),
- status_fetcher_(
- imu_test_event_loop_->MakeFetcher<LocalizerStatus>("/localizer")) {
- localizer_.localizer()->set_longitudinal_offset(0.0);
- aos::TimerHandler *timer = roborio_test_event_loop_->AddTimer([this]() {
- auto builder = output_sender_.MakeBuilder();
- auto output_builder = builder.MakeBuilder<Output>();
- output_builder.add_left_voltage(output_voltages_(0));
- output_builder.add_right_voltage(output_voltages_(1));
- builder.CheckOk(builder.Send(output_builder.Finish()));
- });
- roborio_test_event_loop_->OnRun([timer, this]() {
- timer->Setup(roborio_test_event_loop_->monotonic_now(),
- std::chrono::milliseconds(5));
- });
- // Get things zeroed.
- event_loop_factory_.RunFor(std::chrono::seconds(10));
- CHECK(status_fetcher_.Fetch());
- CHECK(status_fetcher_->zeroed());
- }
-
- aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
- aos::SimulatedEventLoopFactory event_loop_factory_;
- const aos::Node *const roborio_node_;
- const aos::Node *const imu_node_;
- const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
- std::unique_ptr<aos::EventLoop> localizer_event_loop_;
- EventLoopLocalizer localizer_;
-
- std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
- std::unique_ptr<aos::EventLoop> drivetrain_plant_imu_event_loop_;
- control_loops::drivetrain::testing::DrivetrainSimulation drivetrain_plant_;
-
- std::unique_ptr<aos::EventLoop> roborio_test_event_loop_;
- std::unique_ptr<aos::EventLoop> imu_test_event_loop_;
- std::unique_ptr<aos::EventLoop> logger_test_event_loop_;
-
- aos::Sender<Output> output_sender_;
- aos::Fetcher<LocalizerOutput> output_fetcher_;
- aos::Fetcher<LocalizerStatus> status_fetcher_;
-
- Eigen::Vector2d output_voltages_ = Eigen::Vector2d::Zero();
-};
-
-TEST_F(EventLoopLocalizerTest, Nominal) {
- output_voltages_ << 1.0, 1.0;
- event_loop_factory_.RunFor(std::chrono::seconds(2));
- drivetrain_plant_.set_accel_sin_magnitude(0.01);
- CHECK(output_fetcher_.Fetch());
- CHECK(status_fetcher_.Fetch());
- // The two can be different because they may've been sent at different times.
- ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6);
- ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6);
- ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
- 1e-6);
- ASSERT_LT(0.1, output_fetcher_->x());
- ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10);
- ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
- ASSERT_TRUE(status_fetcher_->has_model_based());
- ASSERT_TRUE(status_fetcher_->model_based()->using_model());
- ASSERT_LT(0.1, status_fetcher_->model_based()->accel_state()->velocity_x());
- ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(),
- 1e-10);
- ASSERT_NEAR(
- 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
- 1e-1);
- ASSERT_NEAR(
- 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
- 1e-1);
-}
-
-TEST_F(EventLoopLocalizerTest, Reverse) {
- output_voltages_ << -4.0, -4.0;
- drivetrain_plant_.set_accel_sin_magnitude(0.01);
- event_loop_factory_.RunFor(std::chrono::seconds(2));
- CHECK(output_fetcher_.Fetch());
- CHECK(status_fetcher_.Fetch());
- // The two can be different because they may've been sent at different times.
- ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6);
- ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6);
- ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
- 1e-6);
- ASSERT_GT(-0.1, output_fetcher_->x());
- ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10);
- ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
- ASSERT_TRUE(status_fetcher_->has_model_based());
- ASSERT_TRUE(status_fetcher_->model_based()->using_model());
- ASSERT_GT(-0.1, status_fetcher_->model_based()->accel_state()->velocity_x());
- ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(),
- 1e-10);
- ASSERT_NEAR(
- 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
- 1e-1);
- ASSERT_NEAR(
- 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
- 1e-1);
-}
-
-TEST_F(EventLoopLocalizerTest, SpinInPlace) {
- output_voltages_ << 4.0, -4.0;
- event_loop_factory_.RunFor(std::chrono::seconds(2));
- CHECK(output_fetcher_.Fetch());
- CHECK(status_fetcher_.Fetch());
- // The two can be different because they may've been sent at different times.
- ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6);
- ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6);
- ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
- 1e-1);
- ASSERT_NEAR(0.0, output_fetcher_->x(), 1e-10);
- ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10);
- ASSERT_LT(0.1, std::abs(output_fetcher_->theta()));
- ASSERT_TRUE(status_fetcher_->has_model_based());
- ASSERT_TRUE(status_fetcher_->model_based()->using_model());
- ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_x(),
- 1e-10);
- ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(),
- 1e-10);
- ASSERT_NEAR(-status_fetcher_->model_based()->model_state()->left_velocity(),
- status_fetcher_->model_based()->model_state()->right_velocity(),
- 1e-3);
- ASSERT_NEAR(
- 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
- 1e-1);
- ASSERT_NEAR(
- 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
- 1e-1);
- ASSERT_NEAR(0.0, status_fetcher_->model_based()->residual(), 1e-3);
-}
-
-TEST_F(EventLoopLocalizerTest, Curve) {
- output_voltages_ << 2.0, 4.0;
- event_loop_factory_.RunFor(std::chrono::seconds(2));
- CHECK(output_fetcher_.Fetch());
- CHECK(status_fetcher_.Fetch());
- // The two can be different because they may've been sent at different times.
- ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-2);
- ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-2);
- ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
- 1e-1);
- ASSERT_LT(0.1, output_fetcher_->x());
- ASSERT_LT(0.1, output_fetcher_->y());
- ASSERT_LT(0.1, std::abs(output_fetcher_->theta()));
- ASSERT_TRUE(status_fetcher_->has_model_based());
- ASSERT_TRUE(status_fetcher_->model_based()->using_model());
- ASSERT_LT(0.0, status_fetcher_->model_based()->accel_state()->velocity_x());
- ASSERT_LT(0.0, status_fetcher_->model_based()->accel_state()->velocity_y());
- ASSERT_NEAR(
- 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
- 1e-1);
- ASSERT_NEAR(
- 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
- 1e-1);
- ASSERT_NEAR(0.0, status_fetcher_->model_based()->residual(), 1e-1)
- << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
-}
-
-// Tests that small amounts of voltage error are handled by the model-based
-// half of the localizer.
-TEST_F(EventLoopLocalizerTest, VoltageError) {
- output_voltages_ << 0.0, 0.0;
- drivetrain_plant_.set_left_voltage_offset(2.0);
- drivetrain_plant_.set_right_voltage_offset(2.0);
- drivetrain_plant_.set_accel_sin_magnitude(0.01);
-
- event_loop_factory_.RunFor(std::chrono::seconds(2));
- CHECK(output_fetcher_.Fetch());
- CHECK(status_fetcher_.Fetch());
- // Should still be using the model, but have a non-trivial residual.
- ASSERT_TRUE(status_fetcher_->model_based()->using_model());
- ASSERT_LT(0.02, status_fetcher_->model_based()->residual())
- << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
-
- // Afer running for a while, voltage error terms should converge and result in
- // low residuals.
- event_loop_factory_.RunFor(std::chrono::seconds(10));
- CHECK(output_fetcher_.Fetch());
- CHECK(status_fetcher_.Fetch());
- ASSERT_TRUE(status_fetcher_->model_based()->using_model());
- ASSERT_NEAR(
- 2.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
- 0.1)
- << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
- ASSERT_NEAR(
- 2.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
- 0.1)
- << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
- ASSERT_GT(0.02, status_fetcher_->model_based()->residual())
- << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
-}
-
-// Tests that large amounts of voltage error force us into the
-// acceleration-based localizer.
-TEST_F(EventLoopLocalizerTest, HighVoltageError) {
- output_voltages_ << 0.0, 0.0;
- drivetrain_plant_.set_left_voltage_offset(200.0);
- drivetrain_plant_.set_right_voltage_offset(200.0);
- drivetrain_plant_.set_accel_sin_magnitude(0.01);
-
- event_loop_factory_.RunFor(std::chrono::seconds(2));
- CHECK(output_fetcher_.Fetch());
- CHECK(status_fetcher_.Fetch());
- // Should still be using the model, but have a non-trivial residual.
- ASSERT_FALSE(status_fetcher_->model_based()->using_model());
- ASSERT_LT(0.1, status_fetcher_->model_based()->residual())
- << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
- ASSERT_NEAR(drivetrain_plant_.state()(0),
- status_fetcher_->model_based()->x(), 1.0);
- ASSERT_NEAR(drivetrain_plant_.state()(1),
- status_fetcher_->model_based()->y(), 1e-6);
-}
-
-} // namespace frc91::controls::testing