Merge "Adding scripts for bringup."
diff --git a/aos/controls/control_loop.h b/aos/controls/control_loop.h
index 58f92a2..56353c1 100644
--- a/aos/controls/control_loop.h
+++ b/aos/controls/control_loop.h
@@ -39,18 +39,15 @@
public:
// Create some convenient typedefs to reference the Goal, Position, Status,
// and Output structures.
- typedef typename std::remove_reference<
- decltype(*(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type
- GoalType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type GoalType;
typedef typename std::remove_reference<
decltype(*(static_cast<T *>(NULL)->position.MakeMessage().get()))>::type
- PositionType;
- typedef typename std::remove_reference<
- decltype(*(static_cast<T *>(NULL)->status.MakeMessage().get()))>::type
- StatusType;
- typedef typename std::remove_reference<
- decltype(*(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type
- OutputType;
+ PositionType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<T *>(NULL)->status.MakeMessage().get()))>::type StatusType;
+ typedef typename std::remove_reference<decltype(
+ *(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type OutputType;
ControlLoop(EventLoop *event_loop, const ::std::string &name)
: event_loop_(event_loop), name_(name) {
@@ -102,9 +99,9 @@
protected:
void IteratePosition(const PositionType &position);
- static void Quit(int /*signum*/) {
- run_ = false;
- }
+ EventLoop *event_loop() { return event_loop_; }
+
+ static void Quit(int /*signum*/) { run_ = false; }
// Runs an iteration of the control loop.
// goal is the last goal that was sent. It might be any number of cycles old
@@ -115,10 +112,8 @@
// output is going to be ignored and set to 0.
// status is the status of the control loop.
// Both output and status should be filled in by the implementation.
- virtual void RunIteration(const GoalType *goal,
- const PositionType *position,
- OutputType *output,
- StatusType *status) = 0;
+ virtual void RunIteration(const GoalType *goal, const PositionType *position,
+ OutputType *output, StatusType *status) = 0;
private:
static constexpr ::std::chrono::milliseconds kStaleLogInterval =
diff --git a/aos/controls/control_loop_test.h b/aos/controls/control_loop_test.h
index e88b1b7..66fb174 100644
--- a/aos/controls/control_loop_test.h
+++ b/aos/controls/control_loop_test.h
@@ -80,9 +80,10 @@
}
// Simulates everything that happens during 1 loop time step.
- void SimulateTimestep(bool enabled) {
+ void SimulateTimestep(bool enabled,
+ ::std::chrono::nanoseconds dt = kTimeTick) {
SendMessages(enabled);
- TickTime();
+ TickTime(dt);
}
// Simulate a reset of the process reading sensors, which tells loops that all
diff --git a/aos/events/BUILD b/aos/events/BUILD
index a5e83a9..e0e6bad 100644
--- a/aos/events/BUILD
+++ b/aos/events/BUILD
@@ -73,5 +73,6 @@
deps = [
":event-loop",
"//aos:queues",
+ "//aos/logging",
],
)
diff --git a/aos/events/event-loop.h b/aos/events/event-loop.h
index 0590d7b..90eb721 100644
--- a/aos/events/event-loop.h
+++ b/aos/events/event-loop.h
@@ -13,6 +13,8 @@
class Fetcher {
public:
Fetcher() {}
+ // Fetches the next message. Returns whether it fetched a new message.
+ bool FetchNext() { return fetcher_->FetchNext(); }
// Fetches the most recent message. Returns whether it fetched a new message.
bool Fetch() { return fetcher_->Fetch(); }
diff --git a/aos/events/raw-event-loop.h b/aos/events/raw-event-loop.h
index a903e93..d12187a 100644
--- a/aos/events/raw-event-loop.h
+++ b/aos/events/raw-event-loop.h
@@ -23,6 +23,10 @@
RawFetcher() {}
virtual ~RawFetcher() {}
+ // Non-blocking fetch of the next message in the queue. Returns true if there
+ // was a new message and we got it.
+ virtual bool FetchNext() = 0;
+ // Non-blocking fetch of the latest message:
virtual bool Fetch() = 0;
const FetchValue *most_recent() { return most_recent_; }
diff --git a/aos/events/shm-event-loop.cc b/aos/events/shm-event-loop.cc
index 781c963..a70d6f3 100644
--- a/aos/events/shm-event-loop.cc
+++ b/aos/events/shm-event-loop.cc
@@ -23,7 +23,19 @@
}
}
- bool Fetch() {
+ bool FetchNext() override {
+ const FetchValue *msg = static_cast<const FetchValue *>(
+ queue_->ReadMessageIndex(RawQueue::kNonBlock, &index_));
+ // Only update the internal pointer if we got a new message.
+ if (msg != NULL) {
+ queue_->FreeMessage(msg_);
+ msg_ = msg;
+ set_most_recent(msg_);
+ }
+ return msg != NULL;
+ }
+
+ bool Fetch() override {
static constexpr Options<RawQueue> kOptions =
RawQueue::kFromEnd | RawQueue::kNonBlock;
const FetchValue *msg = static_cast<const FetchValue *>(
diff --git a/aos/events/shm-event-loop_test.cc b/aos/events/shm-event-loop_test.cc
index 544adba..d6f493f 100644
--- a/aos/events/shm-event-loop_test.cc
+++ b/aos/events/shm-event-loop_test.cc
@@ -22,6 +22,54 @@
return new ShmEventLoopTestFactory();
}));
+struct TestMessage : public ::aos::Message {
+ enum { kQueueLength = 100, kHash = 0x696c0cdc };
+ int msg_value;
+
+ void Zero() { msg_value = 0; }
+ static size_t Size() { return 1 + ::aos::Message::Size(); }
+ size_t Print(char *buffer, size_t length) const;
+ TestMessage() { Zero(); }
+};
+
} // namespace
+
+// Tests that FetchNext behaves correctly when we get two messages in the queue
+// but don't consume the first until after the second has been sent.
+// This cannot be abstracted to AbstractEventLoopTest because not all
+// event loops currently support FetchNext().
+TEST(ShmEventLoopTest, FetchNextTest) {
+ ::aos::testing::TestSharedMemory my_shm;
+
+ ShmEventLoop send_loop;
+ ShmEventLoop fetch_loop;
+ auto sender = send_loop.MakeSender<TestMessage>("/test");
+ Fetcher<TestMessage> fetcher = fetch_loop.MakeFetcher<TestMessage>("/test");
+
+ {
+ auto msg = sender.MakeMessage();
+ msg->msg_value = 100;
+ ASSERT_TRUE(msg.Send());
+ }
+
+ {
+ auto msg = sender.MakeMessage();
+ msg->msg_value = 200;
+ ASSERT_TRUE(msg.Send());
+ }
+
+ ASSERT_TRUE(fetcher.FetchNext());
+ ASSERT_NE(nullptr, fetcher.get());
+ EXPECT_EQ(100, fetcher->msg_value);
+
+ ASSERT_TRUE(fetcher.FetchNext());
+ ASSERT_NE(nullptr, fetcher.get());
+ EXPECT_EQ(200, fetcher->msg_value);
+
+ // When we run off the end of the queue, expect to still have the old message:
+ ASSERT_FALSE(fetcher.FetchNext());
+ ASSERT_NE(nullptr, fetcher.get());
+ EXPECT_EQ(200, fetcher->msg_value);
+}
} // namespace testing
} // namespace aos
diff --git a/aos/events/simulated-event-loop.cc b/aos/events/simulated-event-loop.cc
index a23b18c..5fe1bf5 100644
--- a/aos/events/simulated-event-loop.cc
+++ b/aos/events/simulated-event-loop.cc
@@ -2,6 +2,7 @@
#include <algorithm>
+#include "aos/logging/logging.h"
#include "aos/queue.h"
namespace aos {
@@ -11,6 +12,11 @@
explicit SimulatedFetcher(SimulatedQueue *queue) : queue_(queue) {}
~SimulatedFetcher() {}
+ bool FetchNext() override {
+ LOG(FATAL, "Simulated event loops do not support FetchNext.");
+ return false;
+ }
+
bool Fetch() override {
if (index_ == queue_->index()) return false;
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index d95e101..663d36b 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -41,6 +41,40 @@
)
cc_library(
+ name = "hybrid_ekf",
+ hdrs = ["hybrid_ekf.h"],
+ deps = [
+ ":drivetrain_config",
+ "//aos/containers:priority_queue",
+ "//frc971/control_loops:c2d",
+ "//frc971/control_loops:runge_kutta",
+ "//third_party/eigen",
+ ],
+)
+
+cc_test(
+ name = "hybrid_ekf_test",
+ srcs = ["hybrid_ekf_test.cc"],
+ deps = [
+ ":drivetrain_test_lib",
+ ":hybrid_ekf",
+ ":trajectory",
+ "//aos/testing:googletest",
+ "//aos/testing:random_seed",
+ "//aos/testing:test_shm",
+ ],
+)
+
+cc_library(
+ name = "localizer",
+ hdrs = ["localizer.h"],
+ deps = [
+ ":drivetrain_config",
+ ":hybrid_ekf",
+ ],
+)
+
+cc_library(
name = "gear",
hdrs = [
"gear.h",
@@ -57,12 +91,12 @@
"splinedrivetrain.h",
],
deps = [
+ ":distance_spline",
":drivetrain_config",
":drivetrain_queue",
":spline",
- ":distance_spline",
":trajectory",
- ]
+ ],
)
cc_library(
@@ -77,6 +111,7 @@
":drivetrain_config",
":drivetrain_queue",
":gear",
+ ":localizer",
"//aos:math",
"//aos/controls:control_loop",
"//aos/controls:polytope",
@@ -185,9 +220,10 @@
":down_estimator",
":drivetrain_queue",
":gear",
+ ":localizer",
":polydrivetrain",
- ":ssdrivetrain",
":splinedrivetrain",
+ ":ssdrivetrain",
"//aos/controls:control_loop",
"//aos/logging:matrix_logging",
"//aos/logging:queue_logging",
@@ -198,6 +234,23 @@
],
)
+cc_library(
+ name = "drivetrain_test_lib",
+ testonly = True,
+ srcs = ["drivetrain_test_lib.cc"],
+ hdrs = ["drivetrain_test_lib.h"],
+ deps = [
+ ":drivetrain_config",
+ ":drivetrain_queue",
+ ":trajectory",
+ "//aos/testing:googletest",
+ "//frc971/control_loops:state_feedback_loop",
+ "//frc971/queues:gyro",
+ "//y2016:constants",
+ "//y2016/control_loops/drivetrain:polydrivetrain_plants",
+ ],
+)
+
cc_test(
name = "drivetrain_lib_test",
srcs = [
@@ -207,13 +260,11 @@
":drivetrain_config",
":drivetrain_lib",
":drivetrain_queue",
+ ":drivetrain_test_lib",
"//aos:queues",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
- "//frc971/control_loops:state_feedback_loop",
"//frc971/queues:gyro",
- "//y2016:constants",
- "//y2016/control_loops/drivetrain:polydrivetrain_plants",
],
)
@@ -289,6 +340,7 @@
cc_binary(
name = "spline.so",
srcs = ["libspline.cc"],
+ linkshared = True,
deps = [
":distance_spline",
":spline",
@@ -298,7 +350,6 @@
"//third_party/eigen",
"//y2019/control_loops/drivetrain:drivetrain_base",
],
- linkshared=True,
)
cc_test(
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index d097ec8..9cf71f7 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -31,13 +31,15 @@
DrivetrainLoop::DrivetrainLoop(const DrivetrainConfig<double> &dt_config,
::aos::EventLoop *event_loop,
+ LocalizerInterface *localizer,
const ::std::string &name)
: aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
event_loop, name),
dt_config_(dt_config),
+ localizer_(localizer),
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
- dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+ dt_closedloop_(dt_config_, &kf_, localizer_),
dt_spline_(dt_config_),
down_estimator_(MakeDownEstimatorLoop()),
left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
@@ -80,51 +82,6 @@
}
}
-::Eigen::Matrix<double, 3, 1> DrivetrainLoop::PredictState(
- const ::Eigen::Matrix<double, 3, 1> &xytheta_state,
- const ::Eigen::Matrix<double, 7, 1> &state,
- const ::Eigen::Matrix<double, 7, 1> &previous_state) const {
- const double dt =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- dt_config_.dt)
- .count();
-
- const double distance_traveled =
- (state(0) + state(2)) / 2.0 -
- (previous_state(0) + previous_state(2)) / 2.0;
-
- const double omega0 =
- (previous_state(3) - previous_state(1)) / (dt_config_.robot_radius * 2.0);
- const double omega1 = (state(3) - state(1)) / (dt_config_.robot_radius * 2.0);
- const double alpha = (omega1 - omega0) / dt;
-
- const double velocity_start = (previous_state(3) + previous_state(1)) / 2.0;
- const double velocity_end = (state(3) + state(1)) / 2.0;
-
- const double acceleration = (velocity_end - velocity_start) / dt;
- const double velocity_offset =
- distance_traveled / dt - 0.5 * acceleration * dt - velocity_start;
- const double velocity0 = velocity_start + velocity_offset;
-
- // TODO(austin): Substep 10x here. This is super important! ?
- return RungeKutta(
- [&dt, &velocity0, &acceleration, &omega0, &alpha](
- double t, const ::Eigen::Matrix<double, 3, 1> &X) {
- const double velocity1 = velocity0 + acceleration * t;
- const double omega1 = omega0 + alpha * t;
- const double theta = X(2);
-
- return (::Eigen::Matrix<double, 3, 1>()
- << ::std::cos(theta) * velocity1,
- ::std::sin(theta) * velocity1, omega1)
- .finished();
- },
- xytheta_state, 0.0,
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- dt_config_.dt)
- .count());
-}
-
void DrivetrainLoop::RunIteration(
const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
const ::frc971::control_loops::DrivetrainQueue::Position *position,
@@ -274,52 +231,9 @@
Y << position->left_encoder, position->right_encoder, last_gyro_rate_,
last_accel_;
kf_.Correct(Y);
-
- // We are going to choose to integrate velocity to get position by assuming
- // that velocity is a linear function of time. For drivetrains with large
- // amounts of mass, we won't get large changes in acceleration over a 5 ms
- // timestep. Do note, the only place that this matters is when we are
- // talking about the curvature errors introduced by integration. The
- // velocities are scaled such that the distance traveled is correct.
- //
- // We want to do this after the kalman filter runs so we take into account
- // the encoder and gyro corrections.
- //
- // Start by computing the beginning and ending linear and angular
- // velocities.
- // To handle 0 velocity well, compute the offset required to be added to
- // both velocities to make the robot travel the correct distance.
-
- xytheta_state_.block<3, 1>(0, 0) = PredictState(
- xytheta_state_.block<3, 1>(0, 0), kf_.X_hat(), last_state_);
-
- // Use trapezoidal integration for the gyro heading since it's more
- // accurate.
- const double average_angular_velocity =
- ((kf_.X_hat(3) - kf_.X_hat(1)) + (last_state_(3) - last_state_(1))) /
- 2.0 / (dt_config_.robot_radius * 2.0);
-
- integrated_kf_heading_ +=
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- dt_config_.dt)
- .count() *
- average_angular_velocity;
-
- // Copy over the gyro heading.
- xytheta_state_(2) = integrated_kf_heading_;
- // Copy over the velocities heading.
- xytheta_state_(3) = kf_.X_hat(1);
- xytheta_state_(4) = kf_.X_hat(3);
- // Copy over the voltage errors.
- xytheta_state_.block<2, 1>(5, 0) = kf_.X_hat().block<2, 1>(4, 0);
-
- // gyro_heading = (real_right - real_left) / width
- // wheel_heading = (wheel_right - wheel_left) / width
- // gyro_heading + offset = wheel_heading
- // gyro_goal + offset = wheel_goal
- // offset = wheel_heading - gyro_heading
-
- // gyro_goal + wheel_heading - gyro_heading = wheel_goal
+ localizer_->Update({last_left_voltage_, last_right_voltage_}, monotonic_now,
+ position->left_encoder, position->right_encoder,
+ last_gyro_rate_, last_accel_);
}
dt_openloop_.SetPosition(position, left_gear_, right_gear_);
@@ -338,7 +252,10 @@
dt_closedloop_.Update(output != NULL && controller_type == 1);
dt_spline_.Update(output != NULL && controller_type == 2,
- xytheta_state_.block<5, 1>(0, 0));
+ (Eigen::Matrix<double, 5, 1>() << localizer_->x(),
+ localizer_->y(), localizer_->theta(),
+ localizer_->left_velocity(), localizer_->right_velocity())
+ .finished());
switch (controller_type) {
case 0:
@@ -363,7 +280,7 @@
Eigen::Matrix<double, 2, 1> angular =
dt_config_.LeftRightToAngular(kf_.X_hat());
- angular(0, 0) = integrated_kf_heading_;
+ angular(0, 0) = localizer_->theta();
Eigen::Matrix<double, 4, 1> gyro_left_right =
dt_config_.AngularLinearToLeftRight(linear, angular);
@@ -380,11 +297,11 @@
status->left_voltage_error = kf_.X_hat(4);
status->right_voltage_error = kf_.X_hat(5);
status->estimated_angular_velocity_error = kf_.X_hat(6);
- status->estimated_heading = integrated_kf_heading_;
+ status->estimated_heading = localizer_->theta();
- status->x = xytheta_state_(0);
- status->y = xytheta_state_(1);
- status->theta = xytheta_state_(2);
+ status->x = localizer_->x();
+ status->y = localizer_->y();
+ status->theta = localizer_->theta();
status->ground_angle = down_estimator_.X_hat(0) + dt_config_.down_offset;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 05bf711..1aa3e13 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -9,6 +9,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/gear.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
@@ -24,6 +25,7 @@
// drivetrain at frc971::control_loops::drivetrain
explicit DrivetrainLoop(
const DrivetrainConfig<double> &dt_config, ::aos::EventLoop *event_loop,
+ LocalizerInterface *localizer,
const ::std::string &name = ".frc971.control_loops.drivetrain_queue");
int ControllerIndexFromGears();
@@ -38,16 +40,12 @@
void Zero(::frc971::control_loops::DrivetrainQueue::Output *output) override;
- // Computes the xy state change given the change in the lr state.
- ::Eigen::Matrix<double, 3, 1> PredictState(
- const ::Eigen::Matrix<double, 3, 1> &xytheta_state,
- const ::Eigen::Matrix<double, 7, 1> &state,
- const ::Eigen::Matrix<double, 7, 1> &previous_state) const;
-
double last_gyro_rate_ = 0.0;
const DrivetrainConfig<double> dt_config_;
+ LocalizerInterface *localizer_;
+
StateFeedbackLoop<7, 2, 4> kf_;
PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
@@ -65,8 +63,6 @@
double last_left_voltage_ = 0;
double last_right_voltage_ = 0;
- double integrated_kf_heading_ = 0;
-
bool left_high_requested_;
bool right_high_requested_;
@@ -74,12 +70,6 @@
double last_accel_ = 0.0;
- // Current xytheta state of the robot. This is essentially the kalman filter
- // integrated up in a direction.
- // [x, y, theta, vl, vr, left_error, right_error]
- ::Eigen::Matrix<double, 7, 1> xytheta_state_ =
- ::Eigen::Matrix<double, 7, 1>::Zero();
-
// Last kalman filter state.
::Eigen::Matrix<double, 7, 1> last_state_ =
::Eigen::Matrix<double, 7, 1>::Zero();
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 6c16667..363204c 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -5,7 +5,6 @@
#include "aos/controls/control_loop_test.h"
#include "aos/controls/polytope.h"
-#include "aos/network/team_number.h"
#include "aos/time/time.h"
#include "gtest/gtest.h"
@@ -13,14 +12,8 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
-#include "frc971/control_loops/drivetrain/trajectory.h"
-#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/queues/gyro.q.h"
-#include "y2016/constants.h"
-#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
-#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
-#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
namespace frc971 {
namespace control_loops {
@@ -29,215 +22,6 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
-using ::y2016::control_loops::drivetrain::MakeDrivetrainPlant;
-
-// TODO(Comran): Make one that doesn't depend on the actual values for a
-// specific robot.
-const constants::ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25,
- 0.75};
-
-const DrivetrainConfig<double> &GetDrivetrainConfig() {
- static DrivetrainConfig<double> kDrivetrainConfig{
- ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
- ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
- IMUType::IMU_X,
- ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
- ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
- ::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
- ::y2016::control_loops::drivetrain::MakeHybridVelocityDrivetrainLoop,
-
- chrono::duration_cast<chrono::nanoseconds>(
- chrono::duration<double>(::y2016::control_loops::drivetrain::kDt)),
- ::y2016::control_loops::drivetrain::kRobotRadius,
- ::y2016::control_loops::drivetrain::kWheelRadius,
- ::y2016::control_loops::drivetrain::kV,
-
- ::y2016::control_loops::drivetrain::kHighGearRatio,
- ::y2016::control_loops::drivetrain::kLowGearRatio,
- ::y2016::control_loops::drivetrain::kJ,
- ::y2016::control_loops::drivetrain::kMass,
- kThreeStateDriveShifter,
- kThreeStateDriveShifter,
- false,
- 0,
-
- 0.25,
- 1.00,
- 1.00};
-
- return kDrivetrainConfig;
-};
-
-class DrivetrainPlant : public StateFeedbackPlant<4, 2, 2> {
- public:
- explicit DrivetrainPlant(StateFeedbackPlant<4, 2, 2> &&other)
- : StateFeedbackPlant<4, 2, 2>(::std::move(other)) {}
-
- void CheckU(const Eigen::Matrix<double, 2, 1> &U) override {
- EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + left_voltage_offset_);
- EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + left_voltage_offset_);
- EXPECT_LE(U(1, 0), U_max(1, 0) + 0.00001 + right_voltage_offset_);
- EXPECT_GE(U(1, 0), U_min(1, 0) - 0.00001 + right_voltage_offset_);
- }
-
- double left_voltage_offset() const { return left_voltage_offset_; }
- void set_left_voltage_offset(double left_voltage_offset) {
- left_voltage_offset_ = left_voltage_offset;
- }
-
- double right_voltage_offset() const { return right_voltage_offset_; }
- void set_right_voltage_offset(double right_voltage_offset) {
- right_voltage_offset_ = right_voltage_offset;
- }
-
- private:
- double left_voltage_offset_ = 0.0;
- double right_voltage_offset_ = 0.0;
-};
-
-// Class which simulates the drivetrain and sends out queue messages containing
-// the position.
-class DrivetrainSimulation {
- public:
- // Constructs a motor simulation.
- // TODO(aschuh) Do we want to test the clutch one too?
- DrivetrainSimulation()
- : drivetrain_plant_(new DrivetrainPlant(MakeDrivetrainPlant())),
- my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
- ".frc971.control_loops.drivetrain_queue.goal",
- ".frc971.control_loops.drivetrain_queue.position",
- ".frc971.control_loops.drivetrain_queue.output",
- ".frc971.control_loops.drivetrain_queue.status"),
- gyro_reading_(::frc971::sensors::gyro_reading.name()),
- velocity_drivetrain_(::std::unique_ptr<StateFeedbackLoop<
- 2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>>(
- new StateFeedbackLoop<2, 2, 2, double,
- StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>(
- GetDrivetrainConfig()
- .make_hybrid_drivetrain_velocity_loop()))) {
- Reinitialize();
- last_U_.setZero();
- }
-
- // Resets the plant.
- void Reinitialize() {
- drivetrain_plant_->mutable_X(0, 0) = 0.0;
- drivetrain_plant_->mutable_X(1, 0) = 0.0;
- drivetrain_plant_->mutable_Y() =
- drivetrain_plant_->C() * drivetrain_plant_->X();
- last_left_position_ = drivetrain_plant_->Y(0, 0);
- last_right_position_ = drivetrain_plant_->Y(1, 0);
- }
-
- // Returns the position of the drivetrain.
- double GetLeftPosition() const { return drivetrain_plant_->Y(0, 0); }
- double GetRightPosition() const { return drivetrain_plant_->Y(1, 0); }
-
- // Sends out the position queue messages.
- void SendPositionMessage() {
- const double left_encoder = GetLeftPosition();
- const double right_encoder = GetRightPosition();
-
- {
- ::aos::ScopedMessagePtr<
- ::frc971::control_loops::DrivetrainQueue::Position> position =
- my_drivetrain_queue_.position.MakeMessage();
- position->left_encoder = left_encoder;
- position->right_encoder = right_encoder;
- position->left_shifter_position = left_gear_high_ ? 1.0 : 0.0;
- position->right_shifter_position = right_gear_high_ ? 1.0 : 0.0;
- position.Send();
- }
-
- {
- ::aos::ScopedMessagePtr<::frc971::sensors::GyroReading> gyro =
- gyro_reading_.MakeMessage();
- gyro->angle = (right_encoder - left_encoder) /
- (::y2016::control_loops::drivetrain::kRobotRadius * 2.0);
- gyro->velocity =
- (drivetrain_plant_->X(3, 0) - drivetrain_plant_->X(1, 0)) /
- (::y2016::control_loops::drivetrain::kRobotRadius * 2.0);
- gyro.Send();
- }
- }
-
- // Simulates the drivetrain moving for one timestep.
- void Simulate() {
- last_left_position_ = drivetrain_plant_->Y(0, 0);
- last_right_position_ = drivetrain_plant_->Y(1, 0);
- EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
- ::Eigen::Matrix<double, 2, 1> U = last_U_;
- last_U_ << my_drivetrain_queue_.output->left_voltage,
- my_drivetrain_queue_.output->right_voltage;
- {
- ::aos::robot_state.FetchLatest();
- const double scalar = ::aos::robot_state->voltage_battery / 12.0;
- last_U_ *= scalar;
- }
- left_gear_high_ = my_drivetrain_queue_.output->left_high;
- right_gear_high_ = my_drivetrain_queue_.output->right_high;
-
- if (left_gear_high_) {
- if (right_gear_high_) {
- drivetrain_plant_->set_index(3);
- } else {
- drivetrain_plant_->set_index(2);
- }
- } else {
- if (right_gear_high_) {
- drivetrain_plant_->set_index(1);
- } else {
- drivetrain_plant_->set_index(0);
- }
- }
-
- U(0, 0) += drivetrain_plant_->left_voltage_offset();
- U(1, 0) += drivetrain_plant_->right_voltage_offset();
- drivetrain_plant_->Update(U);
- double dt_float =
- ::std::chrono::duration_cast<::std::chrono::duration<double>>(
- GetDrivetrainConfig().dt).count();
- state = RungeKuttaU(
- [this](const ::Eigen::Matrix<double, 5, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
- return ContinuousDynamics(velocity_drivetrain_->plant(),
- GetDrivetrainConfig().Tlr_to_la(), X, U);
- },
- state, U, dt_float);
- }
-
- void set_left_voltage_offset(double left_voltage_offset) {
- drivetrain_plant_->set_left_voltage_offset(left_voltage_offset);
- }
- void set_right_voltage_offset(double right_voltage_offset) {
- drivetrain_plant_->set_right_voltage_offset(right_voltage_offset);
- }
-
- ::std::unique_ptr<DrivetrainPlant> drivetrain_plant_;
-
- ::Eigen::Matrix<double, 2, 1> GetPosition() const {
- return state.block<2,1>(0,0);
- }
-
- private:
- ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
- ::aos::Queue<::frc971::sensors::GyroReading> gyro_reading_;
-
- ::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
- ::std::unique_ptr<
- StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>> velocity_drivetrain_;
- double last_left_position_;
- double last_right_position_;
-
- Eigen::Matrix<double, 2, 1> last_U_;
-
- bool left_gear_high_ = false;
- bool right_gear_high_ = false;
-};
class DrivetrainTest : public ::aos::testing::ControlLoopTest {
protected:
@@ -247,6 +31,8 @@
::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
::aos::ShmEventLoop event_loop_;
+ const DrivetrainConfig<double> dt_config_;
+ DeadReckonEkf localizer_;
// Create a loop and simulation plant.
DrivetrainLoop drivetrain_motor_;
DrivetrainSimulation drivetrain_motor_plant_;
@@ -257,8 +43,10 @@
".frc971.control_loops.drivetrain_queue.position",
".frc971.control_loops.drivetrain_queue.output",
".frc971.control_loops.drivetrain_queue.status"),
- drivetrain_motor_(GetDrivetrainConfig(), &event_loop_),
- drivetrain_motor_plant_() {
+ dt_config_(GetTestDrivetrainConfig()),
+ localizer_(dt_config_),
+ drivetrain_motor_(dt_config_, &event_loop_, &localizer_),
+ drivetrain_motor_plant_(dt_config_) {
::frc971::sensors::gyro_reading.Clear();
set_battery_voltage(12.0);
}
@@ -267,7 +55,7 @@
drivetrain_motor_plant_.SendPositionMessage();
drivetrain_motor_.Iterate();
drivetrain_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SimulateTimestep(true, dt_config_.dt);
}
void RunForTime(monotonic_clock::duration run_for) {
@@ -332,9 +120,9 @@
drivetrain_motor_.Iterate();
drivetrain_motor_plant_.Simulate();
if (i > 20 && i < 200) {
- SimulateTimestep(false);
+ SimulateTimestep(false, dt_config_.dt);
} else {
- SimulateTimestep(true);
+ SimulateTimestep(true, dt_config_.dt);
}
}
VerifyNearGoal();
@@ -394,23 +182,22 @@
// Tests that converting from a left, right position to a distance, angle
// coordinate system and back returns the same answer.
TEST_F(DrivetrainTest, LinearToAngularAndBack) {
- const DrivetrainConfig<double> &dt_config = GetDrivetrainConfig();
- const double width = dt_config.robot_radius * 2.0;
+ const double width = dt_config_.robot_radius * 2.0;
Eigen::Matrix<double, 7, 1> state;
state << 2, 3, 4, 5, 0, 0, 0;
- Eigen::Matrix<double, 2, 1> linear = dt_config.LeftRightToLinear(state);
+ Eigen::Matrix<double, 2, 1> linear = dt_config_.LeftRightToLinear(state);
EXPECT_NEAR(3.0, linear(0, 0), 1e-6);
EXPECT_NEAR(4.0, linear(1, 0), 1e-6);
- Eigen::Matrix<double, 2, 1> angular = dt_config.LeftRightToAngular(state);
+ Eigen::Matrix<double, 2, 1> angular = dt_config_.LeftRightToAngular(state);
EXPECT_NEAR(2.0 / width, angular(0, 0), 1e-6);
EXPECT_NEAR(2.0 / width, angular(1, 0), 1e-6);
Eigen::Matrix<double, 4, 1> back_state =
- dt_config.AngularLinearToLeftRight(linear, angular);
+ dt_config_.AngularLinearToLeftRight(linear, angular);
for (int i = 0; i < 4; ++i) {
EXPECT_NEAR(state(i, 0), back_state(i, 0), 1e-8);
@@ -549,7 +336,7 @@
drivetrain_motor_plant_.SendPositionMessage();
drivetrain_motor_.Iterate();
drivetrain_motor_plant_.Simulate();
- SimulateTimestep(true);
+ SimulateTimestep(true, dt_config_.dt);
ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -6);
EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -6);
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
new file mode 100644
index 0000000..49b0387
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -0,0 +1,191 @@
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+
+#include <chrono>
+
+#include "gtest/gtest.h"
+
+#include "frc971/control_loops/drivetrain/trajectory.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2016/constants.h"
+#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
+#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+namespace {
+// TODO(Comran): Make one that doesn't depend on the actual values for a
+// specific robot.
+const constants::ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25,
+ 0.75};
+
+StateFeedbackPlant<4, 2, 2, double> MakePlantFromConfig(
+ const DrivetrainConfig<double> &dt_config) {
+ ::std::vector<
+ ::std::unique_ptr<StateFeedbackPlantCoefficients<4, 2, 2, double>>>
+ coefs;
+ for (size_t ii = 0;
+ ii < dt_config.make_drivetrain_loop().plant().coefficients_size();
+ ++ii) {
+ coefs.emplace_back(new StateFeedbackPlantCoefficients<4, 2, 2, double>(
+ dt_config.make_drivetrain_loop().plant().coefficients(ii)));
+ }
+ return StateFeedbackPlant<4, 2, 2, double>(&coefs);
+}
+
+} // namespace
+
+namespace chrono = ::std::chrono;
+
+const DrivetrainConfig<double> &GetTestDrivetrainConfig() {
+ static DrivetrainConfig<double> kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
+ IMUType::IMU_X,
+ ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
+ ::y2016::control_loops::drivetrain::MakeHybridVelocityDrivetrainLoop,
+
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(::y2016::control_loops::drivetrain::kDt)),
+ ::y2016::control_loops::drivetrain::kRobotRadius,
+ ::y2016::control_loops::drivetrain::kWheelRadius,
+ ::y2016::control_loops::drivetrain::kV,
+
+ ::y2016::control_loops::drivetrain::kHighGearRatio,
+ ::y2016::control_loops::drivetrain::kLowGearRatio,
+ ::y2016::control_loops::drivetrain::kJ,
+ ::y2016::control_loops::drivetrain::kMass,
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter,
+ false,
+ 0,
+
+ 0.25,
+ 1.00,
+ 1.00};
+
+ return kDrivetrainConfig;
+};
+
+void DrivetrainPlant::CheckU(const Eigen::Matrix<double, 2, 1> &U) {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + left_voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + left_voltage_offset_);
+ EXPECT_LE(U(1, 0), U_max(1, 0) + 0.00001 + right_voltage_offset_);
+ EXPECT_GE(U(1, 0), U_min(1, 0) - 0.00001 + right_voltage_offset_);
+}
+
+DrivetrainSimulation::DrivetrainSimulation(
+ const DrivetrainConfig<double> &dt_config)
+ : dt_config_(dt_config),
+ drivetrain_plant_(MakePlantFromConfig(dt_config_)),
+ my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
+ ".frc971.control_loops.drivetrain_queue.goal",
+ ".frc971.control_loops.drivetrain_queue.position",
+ ".frc971.control_loops.drivetrain_queue.output",
+ ".frc971.control_loops.drivetrain_queue.status"),
+ gyro_reading_(::frc971::sensors::gyro_reading.name()),
+ velocity_drivetrain_(
+ ::std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
+ StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>(
+ new StateFeedbackLoop<2, 2, 2, double,
+ StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>(
+ dt_config_.make_hybrid_drivetrain_velocity_loop()))) {
+ Reinitialize();
+ last_U_.setZero();
+}
+
+void DrivetrainSimulation::Reinitialize() {
+ drivetrain_plant_.mutable_X(0, 0) = 0.0;
+ drivetrain_plant_.mutable_X(1, 0) = 0.0;
+ drivetrain_plant_.mutable_X(2, 0) = 0.0;
+ drivetrain_plant_.mutable_X(3, 0) = 0.0;
+ drivetrain_plant_.mutable_Y() =
+ drivetrain_plant_.C() * drivetrain_plant_.X();
+ last_left_position_ = drivetrain_plant_.Y(0, 0);
+ last_right_position_ = drivetrain_plant_.Y(1, 0);
+}
+
+void DrivetrainSimulation::SendPositionMessage() {
+ const double left_encoder = GetLeftPosition();
+ const double right_encoder = GetRightPosition();
+
+ {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Position>
+ position = my_drivetrain_queue_.position.MakeMessage();
+ position->left_encoder = left_encoder;
+ position->right_encoder = right_encoder;
+ position->left_shifter_position = left_gear_high_ ? 1.0 : 0.0;
+ position->right_shifter_position = right_gear_high_ ? 1.0 : 0.0;
+ position.Send();
+ }
+
+ {
+ ::aos::ScopedMessagePtr<::frc971::sensors::GyroReading> gyro =
+ gyro_reading_.MakeMessage();
+ gyro->angle = (right_encoder - left_encoder) /
+ (dt_config_.robot_radius * 2.0);
+ gyro->velocity = (drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
+ (dt_config_.robot_radius * 2.0);
+ gyro.Send();
+ }
+}
+
+// Simulates the drivetrain moving for one timestep.
+void DrivetrainSimulation::Simulate() {
+ last_left_position_ = drivetrain_plant_.Y(0, 0);
+ last_right_position_ = drivetrain_plant_.Y(1, 0);
+ EXPECT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ ::Eigen::Matrix<double, 2, 1> U = last_U_;
+ last_U_ << my_drivetrain_queue_.output->left_voltage,
+ my_drivetrain_queue_.output->right_voltage;
+ {
+ ::aos::robot_state.FetchLatest();
+ const double scalar = ::aos::robot_state->voltage_battery / 12.0;
+ last_U_ *= scalar;
+ }
+ left_gear_high_ = my_drivetrain_queue_.output->left_high;
+ right_gear_high_ = my_drivetrain_queue_.output->right_high;
+
+ if (left_gear_high_) {
+ if (right_gear_high_) {
+ drivetrain_plant_.set_index(3);
+ } else {
+ drivetrain_plant_.set_index(2);
+ }
+ } else {
+ if (right_gear_high_) {
+ drivetrain_plant_.set_index(1);
+ } else {
+ drivetrain_plant_.set_index(0);
+ }
+ }
+
+ U(0, 0) += drivetrain_plant_.left_voltage_offset();
+ U(1, 0) += drivetrain_plant_.right_voltage_offset();
+ drivetrain_plant_.Update(U);
+ double dt_float =
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ dt_config_.dt)
+ .count();
+ state_ = RungeKuttaU(
+ [this](const ::Eigen::Matrix<double, 5, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return ContinuousDynamics(velocity_drivetrain_->plant(),
+ dt_config_.Tlr_to_la(), X, U);
+ },
+ state_, U, dt_float);
+}
+
+} // namespace testing
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
new file mode 100644
index 0000000..a8dfa07
--- /dev/null
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -0,0 +1,101 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_TEST_LIB_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_TEST_LIB_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/queues/gyro.q.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+const DrivetrainConfig<double> &GetTestDrivetrainConfig();
+
+class DrivetrainPlant : public StateFeedbackPlant<4, 2, 2> {
+ public:
+ explicit DrivetrainPlant(StateFeedbackPlant<4, 2, 2> &&other)
+ : StateFeedbackPlant<4, 2, 2>(::std::move(other)) {}
+
+ void CheckU(const Eigen::Matrix<double, 2, 1> &U) override;
+
+ double left_voltage_offset() const { return left_voltage_offset_; }
+ void set_left_voltage_offset(double left_voltage_offset) {
+ left_voltage_offset_ = left_voltage_offset;
+ }
+
+ double right_voltage_offset() const { return right_voltage_offset_; }
+ void set_right_voltage_offset(double right_voltage_offset) {
+ right_voltage_offset_ = right_voltage_offset;
+ }
+
+ private:
+ double left_voltage_offset_ = 0.0;
+ double right_voltage_offset_ = 0.0;
+};
+
+// Class which simulates the drivetrain and sends out queue messages containing
+// the position.
+class DrivetrainSimulation {
+ public:
+ // Constructs a motor simulation.
+ // TODO(aschuh) Do we want to test the clutch one too?
+ DrivetrainSimulation(const DrivetrainConfig<double> &dt_config);
+
+ // Resets the plant.
+ void Reinitialize();
+
+ // Returns the position of the drivetrain.
+ double GetLeftPosition() const { return drivetrain_plant_.Y(0, 0); }
+ double GetRightPosition() const { return drivetrain_plant_.Y(1, 0); }
+
+ // Sends out the position queue messages.
+ void SendPositionMessage();
+
+ // Simulates the drivetrain moving for one timestep.
+ void Simulate();
+
+ void set_left_voltage_offset(double left_voltage_offset) {
+ drivetrain_plant_.set_left_voltage_offset(left_voltage_offset);
+ }
+ void set_right_voltage_offset(double right_voltage_offset) {
+ drivetrain_plant_.set_right_voltage_offset(right_voltage_offset);
+ }
+
+ Eigen::Matrix<double, 5, 1> state() const { return state_; }
+
+ Eigen::Matrix<double, 5, 1> *mutable_state() { return &state_; }
+
+ ::Eigen::Matrix<double, 2, 1> GetPosition() const {
+ return state_.block<2,1>(0,0);
+ }
+
+ private:
+ DrivetrainConfig<double> dt_config_;
+
+ DrivetrainPlant drivetrain_plant_;
+
+ ::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
+ ::aos::Queue<::frc971::sensors::GyroReading> gyro_reading_;
+
+ // This state is [x, y, theta, left_velocity, right_velocity].
+ ::Eigen::Matrix<double, 5, 1> state_ = ::Eigen::Matrix<double, 5, 1>::Zero();
+ ::std::unique_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>> velocity_drivetrain_;
+ double last_left_position_;
+ double last_right_position_;
+
+ Eigen::Matrix<double, 2, 1> last_U_;
+
+ bool left_gear_high_ = false;
+ bool right_gear_high_ = false;
+};
+
+} // namespace testing
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_TEST_LIB_H_
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
new file mode 100644
index 0000000..01a49a9
--- /dev/null
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -0,0 +1,463 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_HYBRID_EKF_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_HYBRID_EKF_H_
+
+#include <chrono>
+
+#include "aos/containers/priority_queue.h"
+#include "frc971/control_loops/c2d.h"
+#include "frc971/control_loops/runge_kutta.h"
+#include "Eigen/Dense"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace testing {
+class ParameterizedLocalizerTest;
+} // namespace testing
+} // namespace control_loops
+} // namespace y2019
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+namespace testing {
+class HybridEkfTest;
+}
+
+// HybridEkf is an EKF for use in robot localization. It is currently
+// coded for use with drivetrains in particular, and so the states and inputs
+// are chosen as such.
+// The "Hybrid" part of the name refers to the fact that it can take in
+// measurements with variable time-steps.
+// measurements can also have been taken in the past and we maintain a buffer
+// so that we can replay the kalman filter whenever we get an old measurement.
+// Currently, this class provides the necessary utilities for doing
+// measurement updates with an encoder/gyro as well as a more generic
+// update function that can be used for arbitrary nonlinear updates (presumably
+// a camera update).
+template <typename Scalar = double>
+class HybridEkf {
+ public:
+ // An enum specifying what each index in the state vector is for.
+ enum StateIdx {
+ kX = 0,
+ kY = 1,
+ kTheta = 2,
+ kLeftEncoder = 3,
+ kLeftVelocity = 4,
+ kRightEncoder = 5,
+ kRightVelocity = 6,
+ kLeftVoltageError = 7,
+ kRightVoltageError = 8 ,
+ kAngularError = 9,
+ };
+ static constexpr int kNStates = 10;
+ static constexpr int kNInputs = 2;
+ // Number of previous samples to save.
+ static constexpr int kSaveSamples = 50;
+ // Assume that all correction steps will have kNOutputs
+ // dimensions.
+ // TODO(james): Relax this assumption; relaxing it requires
+ // figuring out how to deal with storing variable size
+ // observation matrices, though.
+ static constexpr int kNOutputs = 3;
+ // Inputs are [left_volts, right_volts]
+ typedef Eigen::Matrix<Scalar, kNInputs, 1> Input;
+ // Outputs are either:
+ // [left_encoder, right_encoder, gyro_vel]; or [heading, distance, skew] to
+ // some target. This makes it so we don't have to figure out how we store
+ // variable-size measurement updates.
+ typedef Eigen::Matrix<Scalar, kNOutputs, 1> Output;
+ typedef Eigen::Matrix<Scalar, kNStates, kNStates> StateSquare;
+ // State is [x_position, y_position, theta, Kalman States], where
+ // Kalman States are the states from the standard drivetrain Kalman Filter,
+ // which is: [left encoder, left ground vel, right encoder, right ground vel,
+ // left voltage error, right voltage error, angular_error], where:
+ // left/right encoder should correspond directly to encoder readings
+ // left/right velocities are the velocity of the left/right sides over the
+ // ground (i.e., corrected for angular_error).
+ // voltage errors are the difference between commanded and effective voltage,
+ // used to estimate consistent modelling errors (e.g., friction).
+ // angular error is the difference between the angular velocity as estimated
+ // by the encoders vs. estimated by the gyro, such as might be caused by
+ // wheels on one side of the drivetrain being too small or one side's
+ // wheels slipping more than the other.
+ typedef Eigen::Matrix<Scalar, kNStates, 1> State;
+
+ // Constructs a HybridEkf for a particular drivetrain.
+ // Currently, we use the drivetrain config for modelling constants
+ // (continuous time A and B matrices) and for the noise matrices for the
+ // encoders/gyro.
+ HybridEkf(const DrivetrainConfig<Scalar> &dt_config)
+ : dt_config_(dt_config),
+ velocity_drivetrain_coefficients_(
+ dt_config.make_hybrid_drivetrain_velocity_loop()
+ .plant()
+ .coefficients()) {
+ InitializeMatrices();
+ }
+
+ // Set the initial guess of the state. Can only be called once, and before
+ // any measurement updates have occured.
+ // TODO(james): We may want to actually re-initialize and reset things on
+ // the field. Create some sort of Reset() function.
+ void ResetInitialState(::aos::monotonic_clock::time_point t,
+ const State &state, const StateSquare &P) {
+ observations_.clear();
+ X_hat_ = state;
+ P_ = P;
+ observations_.PushFromBottom(
+ {t,
+ t,
+ X_hat_,
+ P_,
+ Input::Zero(),
+ Output::Zero(),
+ {},
+ [](const State &, const Input &) { return Output::Zero(); },
+ [](const State &) {
+ return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero();
+ },
+ Eigen::Matrix<Scalar, kNOutputs, kNOutputs>::Identity()});
+ }
+
+ // Correct with:
+ // A measurement z at time t with z = h(X_hat, U) + v where v has noise
+ // covariance R.
+ // Input U is applied from the previous timestep until time t.
+ // If t is later than any previous measurements, then U must be provided.
+ // If the measurement falls between two previous measurements, then U
+ // can be provided or not; if U is not provided, then it is filled in based
+ // on an assumption that the voltage was held constant between the time steps.
+ // TODO(james): Is it necessary to explicitly to provide a version with H as a
+ // matrix for linear cases?
+ void Correct(
+ const Output &z, const Input *U,
+ ::std::function<
+ void(const State &, const StateSquare &,
+ ::std::function<Output(const State &, const Input &)> *,
+ ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
+ const State &)> *)> make_h,
+ ::std::function<Output(const State &, const Input &)> h,
+ ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+ dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ aos::monotonic_clock::time_point t);
+
+ // A utility function for specifically updating with encoder and gyro
+ // measurements.
+ void UpdateEncodersAndGyro(const Scalar left_encoder,
+ const Scalar right_encoder, const Scalar gyro_rate,
+ const Input &U,
+ ::aos::monotonic_clock::time_point t) {
+ Output z(left_encoder, right_encoder, gyro_rate);
+ Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
+ R.setZero();
+ R.diagonal() << encoder_noise_, encoder_noise_, gyro_noise_;
+ Correct(z, &U, {}, [this](const State &X, const Input &) {
+ return H_encoders_and_gyro_ * X;
+ },
+ [this](const State &) { return H_encoders_and_gyro_; }, R, t);
+ }
+
+ // Sundry accessor:
+ State X_hat() const { return X_hat_; }
+ Scalar X_hat(long i) const { return X_hat_(i, 0); }
+ StateSquare P() const { return P_; }
+ ::aos::monotonic_clock::time_point latest_t() const {
+ return observations_.top().t;
+ }
+
+ private:
+ struct Observation {
+ // Time when the observation was taken.
+ aos::monotonic_clock::time_point t;
+ // Time that the previous observation was taken:
+ aos::monotonic_clock::time_point prev_t;
+ // Estimate of state at previous observation time t, after accounting for
+ // the previous observation.
+ State X_hat;
+ // Noise matrix corresponding to X_hat_.
+ StateSquare P;
+ // The input applied from previous observation until time t.
+ Input U;
+ // Measurement taken at that time.
+ Output z;
+ // A function to create h and dhdx from a given position/covariance
+ // estimate. This is used by the camera to make it so that we only have to
+ // match targets once.
+ // Only called if h and dhdx are empty.
+ ::std::function<
+ void(const State &, const StateSquare &,
+ ::std::function<Output(const State &, const Input &)> *,
+ ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
+ const State &)> *)> make_h;
+ // A function to calculate the expected output at a given state/input.
+ // TODO(james): For encoders/gyro, it is linear and the function call may
+ // be expensive. Potential source of optimization.
+ ::std::function<Output(const State &, const Input &)> h;
+ // The Jacobian of h with respect to x.
+ // We assume that U has no impact on the Jacobian.
+ // TODO(james): Currently, none of the users of this actually make use of
+ // the ability to have dynamic dhdx (technically, the camera code should
+ // recalculate it to be strictly correct, but I was both too lazy to do
+ // so and it seemed unnecessary). This is a potential source for future
+ // optimizations if function calls are being expensive.
+ ::std::function<
+ Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)> dhdx;
+ // The measurement noise matrix.
+ Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
+
+ // In order to sort the observations in the PriorityQueue object, we
+ // need a comparison function.
+ friend bool operator <(const Observation &l, const Observation &r) {
+ return l.t < r.t;
+ }
+ };
+
+ void InitializeMatrices();
+
+ StateSquare AForState(const State &X) const {
+ StateSquare A_continuous = A_continuous_;
+ const Scalar theta = X(kTheta, 0);
+ const Scalar linear_vel =
+ (X(kLeftVelocity, 0) + X(kRightVelocity, 0)) / 2.0;
+ const Scalar stheta = ::std::sin(theta);
+ const Scalar ctheta = ::std::cos(theta);
+ // X and Y derivatives
+ A_continuous(kX, kTheta) = -stheta * linear_vel;
+ A_continuous(kX, kLeftVelocity) = ctheta / 2.0;
+ A_continuous(kX, kRightVelocity) = ctheta / 2.0;
+ A_continuous(kY, kTheta) = ctheta * linear_vel;
+ A_continuous(kY, kLeftVelocity) = stheta / 2.0;
+ A_continuous(kY, kRightVelocity) = stheta / 2.0;
+ return A_continuous;
+ }
+
+ State DiffEq(const State &X, const Input &U) const {
+ State Xdot = A_continuous_ * X + B_continuous_ * U;
+ // And then we need to add on the terms for the x/y change:
+ const Scalar theta = X(kTheta, 0);
+ const Scalar linear_vel =
+ (X(kLeftVelocity, 0) + X(kRightVelocity, 0)) / 2.0;
+ const Scalar stheta = ::std::sin(theta);
+ const Scalar ctheta = ::std::cos(theta);
+ Xdot(kX, 0) = ctheta * linear_vel;
+ Xdot(kY, 0) = stheta * linear_vel;
+ return Xdot;
+ }
+
+ void PredictImpl(const Input &U, std::chrono::nanoseconds dt, State *state,
+ StateSquare *P) {
+ StateSquare A_c = AForState(*state);
+ StateSquare A_d;
+ Eigen::Matrix<Scalar, kNStates, 0> dummy_B;
+ controls::C2D<Scalar, kNStates, 0>(
+ A_c, Eigen::Matrix<Scalar, kNStates, 0>::Zero(), dt, &A_d, &dummy_B);
+ StateSquare Q_d;
+ controls::DiscretizeQ(Q_continuous_, A_c, dt, &Q_d);
+ *P = A_d * *P * A_d.transpose() + Q_d;
+
+ *state = RungeKuttaU(
+ [this](const State &X,
+ const Input &U) { return DiffEq(X, U); },
+ *state, U,
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ .count());
+ }
+
+ void CorrectImpl(const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ const Output &Z, const Output &expected_Z,
+ const Eigen::Matrix<Scalar, kNOutputs, kNStates> &H,
+ State *state, StateSquare *P) {
+ Output err = Z - expected_Z;
+ Eigen::Matrix<Scalar, kNStates, kNOutputs> PH = *P * H.transpose();
+ Eigen::Matrix<Scalar, kNOutputs, kNOutputs> S = H * PH + R;
+ Eigen::Matrix<Scalar, kNStates, kNOutputs> K = PH * S.inverse();
+ *state = *state + K * err;
+ *P = (StateSquare::Identity() - K * H) * *P;
+ }
+
+ void ProcessObservation(Observation *obs, const std::chrono::nanoseconds dt,
+ State *state, StateSquare *P) {
+ *state = obs->X_hat;
+ *P = obs->P;
+ if (dt.count() != 0) {
+ PredictImpl(obs->U, dt, state, P);
+ }
+ if (!(obs->h && obs->dhdx)) {
+ CHECK(obs->make_h);
+ obs->make_h(*state, *P, &obs->h, &obs->dhdx);
+ }
+ CorrectImpl(obs->R, obs->z, obs->h(*state, obs->U), obs->dhdx(*state),
+ state, P);
+ }
+
+ DrivetrainConfig<Scalar> dt_config_;
+ State X_hat_;
+ StateFeedbackHybridPlantCoefficients<2, 2, 2, Scalar>
+ velocity_drivetrain_coefficients_;
+ StateSquare A_continuous_;
+ StateSquare Q_continuous_;
+ StateSquare P_;
+ Eigen::Matrix<Scalar, kNOutputs, kNStates> H_encoders_and_gyro_;
+ Scalar encoder_noise_, gyro_noise_;
+ Eigen::Matrix<Scalar, kNStates, kNInputs> B_continuous_;
+
+ aos::PriorityQueue<Observation, kSaveSamples, ::std::less<Observation>>
+ observations_;
+
+ friend class testing::HybridEkfTest;
+ friend class ::y2019::control_loops::testing::ParameterizedLocalizerTest;
+}; // class HybridEkf
+
+template <typename Scalar>
+void HybridEkf<Scalar>::Correct(
+ const Output &z, const Input *U,
+ ::std::function<
+ void(const State &, const StateSquare &,
+ ::std::function<Output(const State &, const Input &)> *,
+ ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(
+ const State &)> *)> make_h,
+ ::std::function<Output(const State &, const Input &)> h,
+ ::std::function<Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)>
+ dhdx, const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
+ aos::monotonic_clock::time_point t) {
+ CHECK(!observations_.empty());
+ if (!observations_.full() && t < observations_.begin()->t) {
+ LOG(ERROR,
+ "Dropped an observation that was received before we "
+ "initialized.\n");
+ return;
+ }
+ auto cur_it =
+ observations_.PushFromBottom({t, t, State::Zero(), StateSquare::Zero(),
+ Input::Zero(), z, make_h, h, dhdx, R});
+ if (cur_it == observations_.end()) {
+ LOG(DEBUG,
+ "Camera dropped off of end with time of %fs; earliest observation in "
+ "queue has time of %fs.\n",
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ t.time_since_epoch()).count(),
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ observations_.begin()->t.time_since_epoch()).count());
+ return;
+ }
+
+ // Now we populate any state information that depends on where the
+ // observation was inserted into the queue. X_hat and P must be populated
+ // from the values present in the observation *following* this one in
+ // the queue (note that the X_hat and P that we store in each observation
+ // is the values that they held after accounting for the previous
+ // measurement and before accounting for the time between the previous and
+ // current measurement). If we appended to the end of the queue, then
+ // we need to pull from X_hat_ and P_ specifically.
+ // Furthermore, for U:
+ // -If the observation was inserted at the end, then the user must've
+ // provided U and we use it.
+ // -Otherwise, only grab U if necessary.
+ auto next_it = cur_it;
+ ++next_it;
+ if (next_it == observations_.end()) {
+ cur_it->X_hat = X_hat_;
+ cur_it->P = P_;
+ // Note that if next_it == observations_.end(), then because we already
+ // checked for !observations_.empty(), we are guaranteed to have
+ // valid prev_it.
+ auto prev_it = cur_it;
+ --prev_it;
+ cur_it->prev_t = prev_it->t;
+ // TODO(james): Figure out a saner way of handling this.
+ CHECK(U != nullptr);
+ cur_it->U = *U;
+ } else {
+ cur_it->X_hat = next_it->X_hat;
+ cur_it->P = next_it->P;
+ cur_it->prev_t = next_it->prev_t;
+ next_it->prev_t = cur_it->t;
+ cur_it->U = (U == nullptr) ? next_it->U : *U;
+ }
+ // Now we need to rerun the predict step from the previous to the new
+ // observation as well as every following correct/predict up to the current
+ // time.
+ while (true) {
+ // We use X_hat_ and P_ to store the intermediate states, and then
+ // once we reach the end they will all be up-to-date.
+ ProcessObservation(&*cur_it, cur_it->t - cur_it->prev_t, &X_hat_, &P_);
+ CHECK(X_hat_.allFinite());
+ if (next_it != observations_.end()) {
+ next_it->X_hat = X_hat_;
+ next_it->P = P_;
+ } else {
+ break;
+ }
+ ++cur_it;
+ ++next_it;
+ }
+}
+
+template <typename Scalar>
+void HybridEkf<Scalar>::InitializeMatrices() {
+ A_continuous_.setZero();
+ const Scalar diameter = 2.0 * dt_config_.robot_radius;
+ // Theta derivative
+ A_continuous_(kTheta, kLeftVelocity) = -1.0 / diameter;
+ A_continuous_(kTheta, kRightVelocity) = 1.0 / diameter;
+
+ // Encoder derivatives
+ A_continuous_(kLeftEncoder, kLeftVelocity) = 1.0;
+ A_continuous_(kLeftEncoder, kAngularError) = 1.0;
+ A_continuous_(kRightEncoder, kRightVelocity) = 1.0;
+ A_continuous_(kRightEncoder, kAngularError) = -1.0;
+
+ // Pull velocity derivatives from velocity matrices.
+ // Note that this looks really awkward (doesn't use
+ // Eigen blocks) because someone decided that the full
+ // drivetrain Kalman Filter should half a weird convention.
+ // TODO(james): Support shifting drivetrains with changing A_continuous
+ const auto &vel_coefs = velocity_drivetrain_coefficients_;
+ A_continuous_(kLeftVelocity, kLeftVelocity) = vel_coefs.A_continuous(0, 0);
+ A_continuous_(kLeftVelocity, kRightVelocity) = vel_coefs.A_continuous(0, 1);
+ A_continuous_(kRightVelocity, kLeftVelocity) = vel_coefs.A_continuous(1, 0);
+ A_continuous_(kRightVelocity, kRightVelocity) = vel_coefs.A_continuous(1, 1);
+
+ // Provide for voltage error terms:
+ B_continuous_.setZero();
+ B_continuous_.row(kLeftVelocity) = vel_coefs.B_continuous.row(0);
+ B_continuous_.row(kRightVelocity) = vel_coefs.B_continuous.row(1);
+ A_continuous_.template block<kNStates, kNInputs>(0, 7) = B_continuous_;
+
+ Q_continuous_.setZero();
+ // TODO(james): Improve estimates of process noise--e.g., X/Y noise can
+ // probably be reduced when we are stopped because you rarely jump randomly.
+ // Or maybe it's more appropriate to scale wheelspeed noise with wheelspeed,
+ // since the wheels aren't likely to slip much stopped.
+ Q_continuous_(kX, kX) = 0.005;
+ Q_continuous_(kY, kY) = 0.005;
+ Q_continuous_(kTheta, kTheta) = 0.001;
+ Q_continuous_.template block<7, 7>(3, 3) =
+ dt_config_.make_kf_drivetrain_loop().observer().coefficients().Q;
+
+ P_.setZero();
+ P_.diagonal() << 0.1, 0.1, 0.01, 0.02, 0.01, 0.02, 0.01, 1, 1, 0.03;
+
+ H_encoders_and_gyro_.setZero();
+ // Encoders are stored directly in the state matrix, so are a minor
+ // transform away.
+ H_encoders_and_gyro_(0, kLeftEncoder) = 1.0;
+ H_encoders_and_gyro_(1, kRightEncoder) = 1.0;
+ // Gyro rate is just the difference between right/left side speeds:
+ H_encoders_and_gyro_(2, kLeftVelocity) = -1.0 / diameter;
+ H_encoders_and_gyro_(2, kRightVelocity) = 1.0 / diameter;
+
+ const Eigen::Matrix<Scalar, 4, 4> R_kf_drivetrain =
+ dt_config_.make_kf_drivetrain_loop().observer().coefficients().R;
+ encoder_noise_ = R_kf_drivetrain(0, 0);
+ gyro_noise_ = R_kf_drivetrain(2, 2);
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_HYBRID_EKF_H_
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
new file mode 100644
index 0000000..27119b1
--- /dev/null
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -0,0 +1,429 @@
+#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+
+#include <random>
+
+#include "aos/testing/random_seed.h"
+#include "aos/testing/test_shm.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+typedef HybridEkf<>::StateIdx StateIdx;
+
+
+class HybridEkfTest : public ::testing::Test {
+ public:
+ typedef HybridEkf<>::State State;
+ typedef HybridEkf<>::Input Input;
+ ::aos::testing::TestSharedMemory shm_;
+ HybridEkfTest()
+ : dt_config_(GetTestDrivetrainConfig()),
+ ekf_(dt_config_),
+ t0_(::std::chrono::seconds(0)),
+ velocity_plant_coefs_(dt_config_.make_hybrid_drivetrain_velocity_loop()
+ .plant()
+ .coefficients()) {
+ ekf_.ResetInitialState(t0_, State::Zero(),
+ HybridEkf<>::StateSquare::Identity());
+ }
+
+ protected:
+ const State Update(const State &X, const Input &U) {
+ return RungeKuttaU(
+ ::std::bind(&HybridEkfTest::DiffEq, this, ::std::placeholders::_1,
+ ::std::placeholders::_2),
+ X, U, ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ dt_config_.dt).count());
+ }
+ void CheckDiffEq(const State &X, const Input &U) {
+ // Re-implement dynamics as a sanity check:
+ const double diameter = 2.0 * dt_config_.robot_radius;
+ const double theta = X(StateIdx::kTheta, 0);
+ const double stheta = ::std::sin(theta);
+ const double ctheta = ::std::cos(theta);
+ const double left_vel = X(StateIdx::kLeftVelocity, 0);
+ const double right_vel = X(StateIdx::kRightVelocity, 0);
+ const State Xdot_ekf = DiffEq(X, U);
+ EXPECT_EQ(Xdot_ekf(StateIdx::kX, 0), ctheta * (left_vel + right_vel) / 2.0);
+ EXPECT_EQ(Xdot_ekf(StateIdx::kY, 0), stheta * (left_vel + right_vel) / 2.0);
+ EXPECT_EQ(Xdot_ekf(StateIdx::kTheta, 0), (right_vel - left_vel) / diameter);
+ EXPECT_EQ(Xdot_ekf(StateIdx::kLeftEncoder, 0),
+ left_vel + X(StateIdx::kAngularError, 0));
+ EXPECT_EQ(Xdot_ekf(StateIdx::kRightEncoder, 0),
+ right_vel - X(StateIdx::kAngularError, 0));
+
+ Eigen::Matrix<double, 2, 1> vel_x(X(StateIdx::kLeftVelocity, 0),
+ X(StateIdx::kRightVelocity, 0));
+ Eigen::Matrix<double, 2, 1> expected_vel_X =
+ velocity_plant_coefs_.A_continuous * vel_x +
+ velocity_plant_coefs_.B_continuous *
+ (U + X.middleRows<2>(StateIdx::kLeftVoltageError));
+ EXPECT_EQ(Xdot_ekf(StateIdx::kLeftVelocity, 0), expected_vel_X(0, 0));
+ EXPECT_EQ(Xdot_ekf(StateIdx::kRightVelocity, 0), expected_vel_X(1, 0));
+
+ // Dynamics don't expect error terms to change:
+ EXPECT_EQ(0.0, Xdot_ekf.bottomRows<3>().squaredNorm());
+ }
+ State DiffEq(const State &X, const Input &U) {
+ return ekf_.DiffEq(X, U);
+ }
+
+ // Returns a random value sampled from a normal distribution with a standard
+ // deviation of std and a mean of zero.
+ double Normal(double std) {
+ return normal_(gen_) * std;
+ }
+
+ DrivetrainConfig<double> dt_config_;
+ HybridEkf<> ekf_;
+ ::aos::monotonic_clock::time_point t0_;
+ StateFeedbackHybridPlantCoefficients<2, 2, 2> velocity_plant_coefs_;
+
+ ::std::mt19937 gen_{static_cast<uint32_t>(::aos::testing::RandomSeed())};
+ ::std::normal_distribution<> normal_;
+};
+
+// Tests that the dynamics from DiffEq behave as expected:
+TEST_F(HybridEkfTest, CheckDynamics) {
+ CheckDiffEq(State::Zero(), Input::Zero());
+ CheckDiffEq(State::Zero(), {-5.0, 5.0});
+ CheckDiffEq(State::Zero(), {12.0, -3.0});
+ CheckDiffEq((State() << 100.0, 200.0, M_PI, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
+ 0.3).finished(),
+ {5.0, 6.0});
+ CheckDiffEq((State() << 100.0, 200.0, 2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
+ 0.3).finished(),
+ {5.0, 6.0});
+ CheckDiffEq((State() << 100.0, 200.0, -2.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
+ 0.3).finished(),
+ {5.0, 6.0});
+ // And check that a theta outisde of [-M_PI, M_PI] works.
+ CheckDiffEq((State() << 100.0, 200.0, 200.0, 1.234, 0.5, 1.2, 0.6, 3.0, -4.0,
+ 0.3).finished(),
+ {5.0, 6.0});
+}
+
+// Tests that if we provide a bunch of observations of the position
+// with zero change in time, the state should approach the estimation.
+TEST_F(HybridEkfTest, ZeroTimeCorrect) {
+ HybridEkf<>::Output Z(0.5, 0.5, 1);
+ Eigen::Matrix<double, 3, 10> H;
+ H.setIdentity();
+ auto h = [H](const State &X, const Input &) { return H * X; };
+ auto dhdx = [H](const State &) { return H; };
+ Eigen::Matrix<double, 3, 3> R;
+ R.setIdentity();
+ R *= 1e-3;
+ Input U(0, 0);
+ EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kTheta));
+ const double starting_p_norm = ekf_.P().norm();
+ for (int ii = 0; ii < 100; ++ii) {
+ ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_);
+ }
+ EXPECT_NEAR(Z(0, 0), ekf_.X_hat(StateIdx::kX), 1e-3);
+ EXPECT_NEAR(Z(1, 0), ekf_.X_hat(StateIdx::kY), 1e-3);
+ EXPECT_NEAR(Z(2, 0), ekf_.X_hat(StateIdx::kTheta), 1e-3);
+ const double ending_p_norm = ekf_.P().norm();
+ // Due to corrections, noise should've decreased.
+ EXPECT_LT(ending_p_norm, starting_p_norm * 0.95);
+}
+
+// Tests that prediction steps alone, with no corrections, results in sane
+// results. In order to implement the "no corrections" part of that, we just set
+// H to zero.
+TEST_F(HybridEkfTest, PredictionsAreSane) {
+ HybridEkf<>::Output Z(0, 0, 0);
+ // Use true_X to track what we think the true robot state is.
+ State true_X = ekf_.X_hat();
+ Eigen::Matrix<double, 3, 10> H;
+ H.setZero();
+ auto h = [H](const State &X, const Input &) { return H * X; };
+ auto dhdx = [H](const State &) { return H; };
+ // Provide constant input voltage.
+ Eigen::Matrix<double, 2, 1> U;
+ U << 12.0, 10.0;
+ // The exact value of the noise matrix ix unimportant so long as it is
+ // non-zero.
+ Eigen::Matrix<double, 3, 3> R;
+ R.setIdentity();
+ EXPECT_EQ(0.0, ekf_.X_hat().norm());
+ const double starting_p_norm = ekf_.P().norm();
+ for (int ii = 0; ii < 100; ++ii) {
+ ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_ + dt_config_.dt * (ii + 1));
+ true_X = Update(true_X, U);
+ EXPECT_EQ(true_X, ekf_.X_hat());
+ }
+ // We don't care about precise results, just that they are generally sane:
+ // robot should've travelled forwards and slightly to the right.
+ EXPECT_NEAR(0.9, ekf_.X_hat(StateIdx::kX), 0.1);
+ EXPECT_NEAR(-0.15, ekf_.X_hat(StateIdx::kY), 0.05);
+ EXPECT_NEAR(-0.3, ekf_.X_hat(StateIdx::kTheta), 0.05);
+ EXPECT_NEAR(1.0, ekf_.X_hat(StateIdx::kLeftEncoder), 0.1);
+ EXPECT_NEAR(ekf_.X_hat(StateIdx::kLeftEncoder) * 0.8,
+ ekf_.X_hat(StateIdx::kRightEncoder),
+ ekf_.X_hat(StateIdx::kLeftEncoder) * 0.1);
+ EXPECT_NEAR(2.4, ekf_.X_hat(StateIdx::kLeftVelocity), 0.1);
+ EXPECT_NEAR(ekf_.X_hat(StateIdx::kLeftVelocity) * 0.8,
+ ekf_.X_hat(StateIdx::kRightVelocity),
+ ekf_.X_hat(StateIdx::kLeftVelocity) * 0.1);
+ EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kLeftVoltageError));
+ EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kRightVoltageError));
+ EXPECT_EQ(0.0, ekf_.X_hat(StateIdx::kAngularError));
+ const double ending_p_norm = ekf_.P().norm();
+ // Due to lack of corrections, noise should've increased.
+ EXPECT_GT(ending_p_norm, starting_p_norm * 1.10);
+}
+
+// Parameterize HybridEkf off of the number of prediction-only updates to
+// provide before doing measurements. This is so that we make sure to exercise
+// the corner case when we apply measurements that appear at precisely the
+// oldest time-step (during development, this corner case caused some issues).
+class HybridEkfOldCorrectionsTest
+ : public HybridEkfTest,
+ public ::testing::WithParamInterface<size_t> {};
+
+// Tests that creating an old measurement works, by basically running the
+// previous two tests in reverse (running a series of predictions, and then
+// inserting observation(s) at the start to change everything).
+TEST_P(HybridEkfOldCorrectionsTest, CreateOldCorrection) {
+ HybridEkf<>::Output Z;
+ Z.setZero();
+ Eigen::Matrix<double, 3, 10> H;
+ H.setZero();
+ auto h_zero = [H](const State &X, const Input &) { return H * X; };
+ auto dhdx_zero = [H](const State &) { return H; };
+ Eigen::Matrix<double, 2, 1> U;
+ U << 12.0, 12.0;
+ Eigen::Matrix<double, 3, 3> R;
+ R = R.Identity();
+ EXPECT_EQ(0.0, ekf_.X_hat().norm());
+ // We fill up the buffer to be as full as demanded by the user.
+ const size_t n_predictions = GetParam();
+ for (size_t ii = 0; ii < n_predictions; ++ii) {
+ ekf_.Correct(Z, &U, {}, h_zero, dhdx_zero, R,
+ t0_ + dt_config_.dt * (ii + 1));
+ }
+
+ // Store state and covariance after prediction steps.
+ const State modeled_X_hat = ekf_.X_hat();
+ const double modeled_p_norm = ekf_.P().norm();
+
+ Z << 1, 1, M_PI / 2.0;
+ H.setZero();
+ H(0, 0) = 1;
+ H(1, 1) = 1;
+ H(2, 2) = 1;
+ auto h = [H](const State &X, const Input &) { return H * X; };
+ auto dhdx = [H](const State &) { return H; };
+ R.setZero();
+ R.diagonal() << 1e-5, 1e-5, 1e-5;
+ U.setZero();
+ for (int ii = 0; ii < 20; ++ii) {
+ ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_);
+ }
+ const double corrected_p_norm = ekf_.P().norm();
+ State expected_X_hat = modeled_X_hat;
+ expected_X_hat(0, 0) = Z(0, 0);
+ expected_X_hat(1, 0) = Z(1, 0) + modeled_X_hat(0, 0);
+ expected_X_hat(2, 0) = Z(2, 0);
+ EXPECT_LT((expected_X_hat.topRows<7>() - ekf_.X_hat().topRows<7>()).norm(),
+ 1e-3)
+ << "X_hat: " << ekf_.X_hat() << " expected " << expected_X_hat;
+ // The covariance after the predictions but before the corrections should
+ // be higher than after the corrections are made.
+ EXPECT_GT(modeled_p_norm, corrected_p_norm);
+}
+
+// Ensure that we check kSaveSamples - 1, for potential corner cases.
+INSTANTIATE_TEST_CASE_P(OldCorrectionTest, HybridEkfOldCorrectionsTest,
+ ::testing::Values(0, 1, 10,
+ HybridEkf<>::kSaveSamples - 1));
+
+// Tests that creating a correction that is too old results in the correction
+// being dropped and ignored.
+TEST_F(HybridEkfTest, DiscardTooOldCorrection) {
+ HybridEkf<>::Output Z;
+ Z.setZero();
+ Eigen::Matrix<double, 3, 10> H;
+ H.setZero();
+ auto h_zero = [H](const State &X, const Input &) { return H * X; };
+ auto dhdx_zero = [H](const State &) { return H; };
+ Eigen::Matrix<double, 2, 1> U(12.0, 12.0);
+ Eigen::Matrix<double, 3, 3> R;
+ R.setIdentity();
+
+ EXPECT_EQ(0.0, ekf_.X_hat().norm());
+ for (int ii = 0; ii < HybridEkf<>::kSaveSamples; ++ii) {
+ ekf_.Correct(Z, &U, {}, h_zero, dhdx_zero, R,
+ t0_ + dt_config_.dt * (ii + 1));
+ }
+ const State modeled_X_hat = ekf_.X_hat();
+ const HybridEkf<>::StateSquare modeled_P = ekf_.P();
+
+ Z << 1, 1, M_PI / 2.0;
+ H.setZero();
+ H(0, 0) = 1;
+ H(1, 1) = 1;
+ H(2, 2) = 1;
+ auto h = [H](const State &X, const Input &) { return H * X; };
+ auto dhdx = [H](const State &) { return H; };
+ R.setIdentity();
+ R *= 1e-5;
+ U.setZero();
+ ekf_.Correct(Z, &U, {}, h, dhdx, R, t0_);
+ EXPECT_EQ(ekf_.X_hat(), modeled_X_hat)
+ << "Expected too-old correction to have no effect; X_hat: "
+ << ekf_.X_hat() << " expected " << modeled_X_hat;
+ EXPECT_EQ(ekf_.P(), modeled_P)
+ << "Expected too-old correction to have no effect; P: " << ekf_.P()
+ << " expected " << modeled_P;
+}
+
+// Tests The UpdateEncodersAndGyro function works when perfect measurements are
+// provided:
+TEST_F(HybridEkfTest, PerfectEncoderUpdate) {
+ State true_X = ekf_.X_hat();
+ Input U(-1.0, 5.0);
+ for (int ii = 0; ii < 100; ++ii) {
+ true_X = Update(true_X, U);
+ ekf_.UpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
+ true_X(StateIdx::kRightEncoder, 0),
+ (true_X(StateIdx::kRightVelocity, 0) -
+ true_X(StateIdx::kLeftVelocity, 0)) /
+ dt_config_.robot_radius / 2.0,
+ U, t0_ + (ii + 1) * dt_config_.dt);
+ EXPECT_NEAR((true_X - ekf_.X_hat()).squaredNorm(), 0.0, 1e-25)
+ << "Expected only floating point precision errors in update step. "
+ "Estimated X_hat:\n" << ekf_.X_hat() << "\ntrue X:\n" << true_X;
+ }
+}
+
+// Tests that encoder updates cause everything to converge properly in the
+// presence of voltage error.
+TEST_F(HybridEkfTest, PerfectEncoderUpdatesWithVoltageError) {
+ State true_X = ekf_.X_hat();
+ true_X(StateIdx::kLeftVoltageError, 0) = 2.0;
+ true_X(StateIdx::kRightVoltageError, 0) = 2.0;
+ Input U(5.0, 5.0);
+ for (int ii = 0; ii < 1000; ++ii) {
+ true_X = Update(true_X, U);
+ ekf_.UpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
+ true_X(StateIdx::kRightEncoder, 0),
+ (true_X(StateIdx::kRightVelocity, 0) -
+ true_X(StateIdx::kLeftVelocity, 0)) /
+ dt_config_.robot_radius / 2.0,
+ U, t0_ + (ii + 1) * dt_config_.dt);
+ }
+ EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 5e-3)
+ << "Expected non-x/y estimates to converge to correct. "
+ "Estimated X_hat:\n"
+ << ekf_.X_hat() << "\ntrue X:\n"
+ << true_X;
+}
+
+// Tests encoder/gyro updates when we have some errors in our estimate.
+TEST_F(HybridEkfTest, PerfectEncoderUpdateConverges) {
+ // In order to simulate modelling errors, we add an angular_error and start
+ // the encoder values slightly off.
+ State true_X = ekf_.X_hat();
+ true_X(StateIdx::kAngularError, 0) = 1.0;
+ true_X(StateIdx::kLeftEncoder, 0) += 2.0;
+ true_X(StateIdx::kRightEncoder, 0) -= 2.0;
+ // After enough time, everything should converge to near-perfect (if there
+ // were any errors in the original absolute state (x/y/theta) state, then we
+ // can't correct for those).
+ // Note: Because we don't have any absolute measurements used for corrections,
+ // we will get slightly off on the absolute x/y/theta, but the errors are so
+ // small they are negligible.
+ Input U(10.0, 5.0);
+ for (int ii = 0; ii < 100; ++ii) {
+ true_X = Update(true_X, U);
+ ekf_.UpdateEncodersAndGyro(true_X(StateIdx::kLeftEncoder, 0),
+ true_X(StateIdx::kRightEncoder, 0),
+ (true_X(StateIdx::kRightVelocity, 0) -
+ true_X(StateIdx::kLeftVelocity, 0)) /
+ dt_config_.robot_radius / 2.0,
+ U, t0_ + (ii + 1) * dt_config_.dt);
+ }
+ EXPECT_NEAR((true_X - ekf_.X_hat()).norm(), 0.0, 1e-5)
+ << "Expected non-x/y estimates to converge to correct. "
+ "Estimated X_hat:\n"
+ << ekf_.X_hat() << "\ntrue X:\n"
+ << true_X;
+}
+
+// Tests encoder/gyro updates in a realistic-ish scenario with noise:
+TEST_F(HybridEkfTest, RealisticEncoderUpdateConverges) {
+ // In order to simulate modelling errors, we add an angular_error and start
+ // the encoder values slightly off.
+ State true_X = ekf_.X_hat();
+ true_X(StateIdx::kAngularError, 0) = 1.0;
+ true_X(StateIdx::kLeftEncoder, 0) += 2.0;
+ true_X(StateIdx::kRightEncoder, 0) -= 2.0;
+ Input U(10.0, 5.0);
+ for (int ii = 0; ii < 100; ++ii) {
+ true_X = Update(true_X, U);
+ ekf_.UpdateEncodersAndGyro(
+ true_X(StateIdx::kLeftEncoder, 0) + Normal(1e-3),
+ true_X(StateIdx::kRightEncoder, 0) + Normal(1e-3),
+ (true_X(StateIdx::kRightVelocity, 0) -
+ true_X(StateIdx::kLeftVelocity, 0)) /
+ dt_config_.robot_radius / 2.0 + Normal(1e-4),
+ U, t0_ + (ii + 1) * dt_config_.dt);
+ }
+ EXPECT_NEAR(
+ (true_X.bottomRows<9>() - ekf_.X_hat().bottomRows<9>()).squaredNorm(),
+ 0.0, 2e-3)
+ << "Expected non-x/y estimates to converge to correct. "
+ "Estimated X_hat:\n" << ekf_.X_hat() << "\ntrue X:\n" << true_X;
+}
+
+class HybridEkfDeathTest : public HybridEkfTest {};
+
+TEST_F(HybridEkfDeathTest, DieIfUninitialized) {
+ HybridEkf<> ekf(dt_config_);
+ // Expect death if we fail to initialize before starting to provide updates.
+ EXPECT_DEATH(ekf.UpdateEncodersAndGyro(0.0, 1.0, 2.0, {3.0, 3.0}, t0_),
+ "observations_.empty()");
+}
+
+TEST_F(HybridEkfDeathTest, DieOnNoU) {
+ // Expect death if the user does not provide U when creating a fresh
+ // measurement.
+ EXPECT_DEATH(ekf_.Correct({1, 2, 3}, nullptr, {}, {}, {}, {},
+ t0_ + ::std::chrono::seconds(1)),
+ "U != nullptr");
+}
+
+// Because the user can choose to provide only one of make_h or (h, dhdx), check
+// that we die when an improper combination is provided.
+TEST_F(HybridEkfDeathTest, DieOnNoH) {
+ // Check that we die when no h-related functions are provided:
+ Input U(1, 2);
+ EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {}, {}, {},
+ t0_ + ::std::chrono::seconds(1)),
+ "make_h");
+ // Check that we die when only one of h and dhdx are provided:
+ EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {}, {},
+ [](const State &) {
+ return Eigen::Matrix<double, 3, 10>::Zero();
+ },
+ {}, t0_ + ::std::chrono::seconds(1)),
+ "make_h");
+ EXPECT_DEATH(ekf_.Correct({1, 2, 3}, &U, {},
+ [](const State &, const Input &) {
+ return Eigen::Matrix<double, 3, 1>::Zero();
+ },
+ {}, {}, t0_ + ::std::chrono::seconds(1)),
+ "make_h");
+}
+
+} // namespace testing
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
new file mode 100644
index 0000000..4da8b57
--- /dev/null
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -0,0 +1,83 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+// Defines an interface for classes that provide field-global localization.
+class LocalizerInterface {
+ public:
+ // Perform a single step of the filter, using the information that is
+ // available on every drivetrain iteration.
+ // The user should pass in the U that the real system experienced from the
+ // previous timestep until now; internally, any filters will first perform a
+ // prediction step to get the estimate at time now, and then will apply
+ // corrections based on the encoder/gyro/accelerometer values from time now.
+ // TODO(james): Consider letting implementations subscribe to the sensor
+ // values themselves, and then only passing in U. This requires more
+ // coordination on timing, however.
+ virtual void Update(const ::Eigen::Matrix<double, 2, 1> &U,
+ ::aos::monotonic_clock::time_point now,
+ double left_encoder, double right_encoder,
+ double gyro_rate, double longitudinal_accelerometer) = 0;
+ // There are several subtly different norms floating around for state
+ // matrices. In order to avoid that mess, we jus tprovide direct accessors for
+ // the values that most people care about.
+ virtual double x() const = 0;
+ virtual double y() const = 0;
+ virtual double theta() const = 0;
+ virtual double left_velocity() const = 0;
+ virtual double right_velocity() const = 0;
+ virtual double left_voltage_error() const = 0;
+ virtual double right_voltage_error() const = 0;
+};
+
+// Uses the generic HybridEkf implementation to provide a basic field estimator.
+// This provides no method for using cameras or the such to get global
+// measurements and just assumes that you can dead-reckon perfectly.
+class DeadReckonEkf : public LocalizerInterface {
+ typedef HybridEkf<double> Ekf;
+ typedef typename Ekf::StateIdx StateIdx;
+ public:
+ DeadReckonEkf(const DrivetrainConfig<double> &dt_config) : ekf_(dt_config) {
+ ekf_.ResetInitialState(::aos::monotonic_clock::now(), Ekf::State::Zero(),
+ ekf_.P());
+ }
+
+ void Update(const ::Eigen::Matrix<double, 2, 1> &U,
+ ::aos::monotonic_clock::time_point now,
+ double left_encoder, double right_encoder,
+ double gyro_rate,
+ double /*longitudinal_accelerometer*/) override {
+ ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U, now);
+ }
+
+ double x() const override { return ekf_.X_hat(StateIdx::kX); }
+ double y() const override { return ekf_.X_hat(StateIdx::kY); }
+ double theta() const override { return ekf_.X_hat(StateIdx::kTheta); }
+ double left_velocity() const override {
+ return ekf_.X_hat(StateIdx::kLeftVelocity);
+ }
+ double right_velocity() const override {
+ return ekf_.X_hat(StateIdx::kRightVelocity);
+ }
+ double left_voltage_error() const override {
+ return ekf_.X_hat(StateIdx::kLeftVoltageError);
+ }
+ double right_voltage_error() const override {
+ return ekf_.X_hat(StateIdx::kRightVoltageError);
+ }
+
+ private:
+ Ekf ekf_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_FIELD_ESTIMATOR_H_
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index b45f218..9aa1708 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -88,15 +88,14 @@
enable_ = enable;
if (enable && current_trajectory_) {
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
- if (!current_trajectory_->is_at_end(current_state_)) {
+ if (!IsAtEnd()) {
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
U_ff = current_trajectory_->FFVoltage(current_xva_(0));
}
::Eigen::Matrix<double, 2, 5> K =
current_trajectory_->KForState(state, dt_config_.dt, Q, R);
- ::Eigen::Matrix<double, 5, 1> goal_state =
- current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+ ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
next_U_ = U_ff + U_fb;
@@ -116,7 +115,7 @@
return;
}
if (current_spline_handle_ == current_spline_idx_) {
- if (!current_trajectory_->is_at_end(current_state_)) {
+ if (!IsAtEnd()) {
output->left_voltage = next_U_(0);
output->right_voltage = next_U_(1);
current_xva_ = next_xva_;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index a065167..2a79df6 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -26,6 +26,16 @@
// TODO(alex): What status do we need?
void PopulateStatus(
::frc971::control_loops::DrivetrainQueue::Status *status) const;
+
+ // Accessor for the current goal state, pretty much only present for debugging
+ // purposes.
+ Eigen::Matrix<double, 5, 1> CurrentGoalState() const {
+ return current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+ }
+
+ bool IsAtEnd() const {
+ return current_trajectory_->is_at_end(current_state_);
+ }
private:
void ScaleCapU(Eigen::Matrix<double, 2, 1> *U);
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 292f291..5330414 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -15,7 +15,7 @@
DrivetrainMotorsSS::DrivetrainMotorsSS(
const DrivetrainConfig<double> &dt_config, StateFeedbackLoop<7, 2, 4> *kf,
- double *integrated_kf_heading)
+ LocalizerInterface *localizer)
: dt_config_(dt_config),
kf_(kf),
U_poly_(
@@ -34,7 +34,7 @@
.finished()),
linear_profile_(::aos::controls::kLoopFrequency),
angular_profile_(::aos::controls::kLoopFrequency),
- integrated_kf_heading_(integrated_kf_heading) {
+ localizer_(localizer) {
::aos::controls::HPolytope<0>::Init();
T_ << 1, 1, 1, -1;
T_inverse_ = T_.inverse();
@@ -168,8 +168,7 @@
Eigen::Matrix<double, 2, 1> wheel_heading =
dt_config_.LeftRightToAngular(kf_->X_hat());
- const double gyro_to_wheel_offset =
- wheel_heading(0, 0) - *integrated_kf_heading_;
+ const double gyro_to_wheel_offset = wheel_heading(0, 0) - localizer_->theta();
if (enable_control_loop) {
// Update profiles.
@@ -240,7 +239,7 @@
Eigen::Matrix<double, 2, 1> wheel_linear =
dt_config_.LeftRightToLinear(kf_->X_hat());
Eigen::Matrix<double, 2, 1> next_angular = wheel_heading;
- next_angular(0, 0) = *integrated_kf_heading_;
+ next_angular(0, 0) = localizer_->theta();
unprofiled_goal_.block<4, 1>(0, 0) =
dt_config_.AngularLinearToLeftRight(wheel_linear, next_angular);
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index ee8d145..762cbc2 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -11,6 +11,7 @@
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
namespace frc971 {
namespace control_loops {
@@ -20,7 +21,7 @@
public:
DrivetrainMotorsSS(const DrivetrainConfig<double> &dt_config,
StateFeedbackLoop<7, 2, 4> *kf,
- double *integrated_kf_heading);
+ LocalizerInterface *localizer);
void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
@@ -65,7 +66,7 @@
bool use_profile_ = false;
- double *integrated_kf_heading_;
+ LocalizerInterface *localizer_;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index ac22821..8bd70fe 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -598,6 +598,7 @@
self.resistance = motor.resistance / n
self.Kv = motor.Kv
self.Kt = motor.Kt
+ self.motor_inertia = motor.motor_inertia * n
class Vex775Pro(object):
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 0da8bd6..d5861b7 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -115,6 +115,8 @@
Eigen::Matrix<Scalar, number_of_outputs, 1> &mutable_Y() { return Y_; }
Scalar &mutable_Y(int i, int j = 0) { return mutable_Y()(i, j); }
+ size_t coefficients_size() const { return coefficients_.size(); }
+
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
number_of_outputs, Scalar>
&coefficients(int index) const {
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 635db59..2951f1a 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -72,6 +72,8 @@
// Resets the profiled subsystem and returns to uninitialized
void Reset();
+ void TriggerEstimatorError() { profiled_subsystem_.TriggerEstimatorError(); }
+
enum class State : int32_t {
UNINITIALIZED,
DISABLED_INITIALIZED,
@@ -127,6 +129,10 @@
bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
+ if (profiled_subsystem_.error()) {
+ state_ = State::ESTOP;
+ }
+
switch (state_) {
case State::UNINITIALIZED:
if (profiled_subsystem_.initialized()) {
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 66b1ab9..f685e1b 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -498,11 +498,22 @@
this->VerifyNearGoal();
}
+// Tests that the subsystem estops when a zeroing error occurs
+TYPED_TEST_P(IntakeSystemTest, ZeroingErrorTest) {
+ this->RunForTime(chrono::seconds(2), true);
+
+ EXPECT_EQ(this->subsystem_.state(), TestFixture::SZSDPS::State::RUNNING);
+ this->subsystem_.TriggerEstimatorError();
+ this->RunIteration(true, false);
+ EXPECT_EQ(this->subsystem_.state(), TestFixture::SZSDPS::State::ESTOP);
+}
+
REGISTER_TYPED_TEST_CASE_P(IntakeSystemTest, DoesNothing, ReachesGoal,
SaturationTest, RespectsRange, ZeroTest, ZeroNoGoal,
LowerHardstopStartup, UpperHardstopStartup,
ResetTest, DisabledGoalTest, DisabledZeroTest,
- MinPositionTest, MaxPositionTest, NullGoalTest);
+ MinPositionTest, MaxPositionTest, NullGoalTest,
+ ZeroingErrorTest);
INSTANTIATE_TYPED_TEST_CASE_P(My, IntakeSystemTest, TestTypes);
} // namespace control_loops
diff --git a/tools/bazel b/tools/bazel
index a691066..d305cf5 100755
--- a/tools/bazel
+++ b/tools/bazel
@@ -33,7 +33,7 @@
# Creating might fail if another invocation is racing us.
if [[ ! -d "${DOWNLOAD_DIR}" ]]; then
- mkdir "${DOWNLOAD_DIR}" || true
+ mkdir -p "${DOWNLOAD_DIR}" || true
fi
if [[ ! -d "${DOWNLOAD_DIR}" ]]; then
echo "Failed to create ${DOWNLOAD_DIR}" >&2
diff --git a/tools/ci/run-tests.sh b/tools/ci/run-tests.sh
index 4688378..24cdc5b 100755
--- a/tools/ci/run-tests.sh
+++ b/tools/ci/run-tests.sh
@@ -2,12 +2,32 @@
set -e
set -x
-TARGETS='//... @com_github_google_glog//... @com_google_ceres_solver//...'
+readonly TARGETS='//... @com_github_google_glog//... @com_google_ceres_solver//...'
+readonly M4F_TARGETS='//motors/... //y2019/jevois/...'
+readonly COMMON='-c opt --stamp=no --curses=no --color=no --symlink_prefix=/'
+
+# Put everything in different output bases so we can get 4 bazel servers
+# running and keep them all warm.
# Include --config=eigen to enable Eigen assertions so that we catch potential
# bugs with Eigen.
-bazel test -c opt --stamp=no --config=eigen --curses=no --color=no ${TARGETS}
-bazel build -c opt --stamp=no --curses=no --color=no ${TARGETS} --cpu=roborio
-bazel build --stamp=no --curses=no --color=no ${TARGETS} --cpu=armhf-debian
-bazel build -c opt --stamp=no --curses=no --color=no \
- //motors/... //y2019/jevois/... --cpu=cortex-m4f
+bazel --output_base=../k8_output_base test \
+ ${COMMON} \
+ --cpu=k8 \
+ --config=eigen \
+ ${TARGETS}
+
+bazel --output_base=../roborio_output_base build \
+ ${COMMON} \
+ --cpu=roborio \
+ ${TARGETS}
+
+bazel --output_base=../armhf-debian_output_base build \
+ ${COMMON} \
+ --cpu=armhf-debian \
+ ${TARGETS}
+
+bazel --output_base=../cortex-m4f_output_base build \
+ ${COMMON} \
+ --cpu=cortex-m4f \
+ ${M4F_TARGETS}
diff --git a/y2012/control_loops/drivetrain/drivetrain_main.cc b/y2012/control_loops/drivetrain/drivetrain_main.cc
index ef348a4..6e5b845 100644
--- a/y2012/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2012/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2012::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
- ::y2012::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
+ ::y2012::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014/control_loops/drivetrain/drivetrain_main.cc b/y2014/control_loops/drivetrain/drivetrain_main.cc
index 19c2659..21dfcd2 100644
--- a/y2014/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,10 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2014::control_loops::GetDrivetrainConfig());
DrivetrainLoop drivetrain(::y2014::control_loops::GetDrivetrainConfig(),
- &event_loop);
+ &event_loop, &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
index a76b5a3..f13a8ab 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -9,9 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2014_bot3::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2014_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
- &event_loop);
+ &event_loop, &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2016/control_loops/drivetrain/drivetrain_main.cc b/y2016/control_loops/drivetrain/drivetrain_main.cc
index e74ddea..12d8642 100644
--- a/y2016/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2016/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2016::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
- ::y2016::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
+ ::y2016::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2017/control_loops/drivetrain/drivetrain_main.cc b/y2017/control_loops/drivetrain/drivetrain_main.cc
index ffe479a..808f215 100644
--- a/y2017/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2017/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2017::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
- ::y2017::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
+ ::y2017::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2018/control_loops/drivetrain/drivetrain_main.cc b/y2018/control_loops/drivetrain/drivetrain_main.cc
index f0c7f0e..038497a 100644
--- a/y2018/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2018/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2018::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
- ::y2018::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
+ ::y2018::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc b/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
index e481f6c..c768dc9 100644
--- a/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2018_bot3/control_loops/drivetrain/drivetrain_main.cc
@@ -9,9 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2018_bot3::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
::y2018_bot3::control_loops::drivetrain::GetDrivetrainConfig(),
- &event_loop);
+ &event_loop, &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2019/BUILD b/y2019/BUILD
index 14d0d6f..a4f498b 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -86,6 +86,7 @@
],
deps = [
"//aos/input:drivetrain_input",
+ "//frc971/zeroing:wrap",
],
)
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 791a6c4..d77d2c7 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -104,6 +104,7 @@
intake->zeroing_constants.zeroing_threshold = 0.0005;
intake->zeroing_constants.moving_buffer_size = 20;
intake->zeroing_constants.allowable_encoder_error = 0.9;
+ intake->zeroing_constants.middle_position = Values::kIntakeRange().middle();
// Stilts constants.
stilts_params->zeroing_voltage = 3.0;
@@ -128,7 +129,6 @@
elevator->potentiometer_offset = 0.0;
intake->zeroing_constants.measured_absolute_position = 0.0;
- intake->zeroing_constants.middle_position = 0.0;
wrist_params->zeroing_constants.measured_absolute_position = 0.0;
wrist->potentiometer_offset = 0.0;
@@ -138,35 +138,29 @@
break;
case kCompTeamNumber:
- elevator_params->zeroing_constants.measured_absolute_position = 0.0;
- elevator->potentiometer_offset = 0.0;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.019470;
+ elevator->potentiometer_offset = -0.075017;
- intake->zeroing_constants.measured_absolute_position = 0.0;
- intake->zeroing_constants.middle_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 1.860016;
- wrist_params->zeroing_constants.measured_absolute_position = 0.0;
- wrist->potentiometer_offset = 0.0;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.163840;
+ wrist->potentiometer_offset = -4.257454;
- stilts_params->zeroing_constants.measured_absolute_position = 0.0;
- stilts->potentiometer_offset = 0.0;
+ stilts_params->zeroing_constants.measured_absolute_position = 0.030049;
+ stilts->potentiometer_offset = -0.015760 + 0.011604;
break;
case kPracticeTeamNumber:
- elevator_params->zeroing_constants.measured_absolute_position = 0.049419;
- elevator->potentiometer_offset = -0.022320;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.160767;
+ elevator->potentiometer_offset = -0.022320 + 0.020567 - 0.022355;
intake->zeroing_constants.measured_absolute_position = 1.756847;
- intake->zeroing_constants.middle_position =
- Values::kIntakeRange().middle();
-
- stilts_params->zeroing_constants.measured_absolute_position = 0.0;
- stilts->potentiometer_offset = 0.0;
wrist_params->zeroing_constants.measured_absolute_position = 0.357394;
wrist->potentiometer_offset = -1.479097 - 2.740303;
- stilts_params->zeroing_constants.measured_absolute_position = 0.047838;
- stilts->potentiometer_offset = -0.093820;
+ stilts_params->zeroing_constants.measured_absolute_position = 0.036469;
+ stilts->potentiometer_offset = -0.093820 + 0.0124;
break;
case kCodingRobotTeamNumber:
@@ -174,7 +168,6 @@
elevator->potentiometer_offset = 0.0;
intake->zeroing_constants.measured_absolute_position = 0.0;
- intake->zeroing_constants.middle_position = 0.0;
wrist_params->zeroing_constants.measured_absolute_position = 0.0;
wrist->potentiometer_offset = 0.0;
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 38c73f0..310b853 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -1,4 +1,5 @@
load("//aos/build:queues.bzl", "queue_library")
+load("//tools/build_rules:select.bzl", "cpu_select", "compiler_select")
genrule(
name = "genrule_drivetrain",
@@ -99,3 +100,42 @@
"//aos/testing:googletest",
],
)
+
+cc_library(
+ name = "localizer",
+ hdrs = ["localizer.h"],
+ deps = [
+ ":camera",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops/drivetrain:hybrid_ekf",
+ ],
+)
+
+cc_test(
+ name = "localizer_test",
+ srcs = ["localizer_test.cc"],
+ defines =
+ cpu_select({
+ "amd64": [
+ "SUPPORT_PLOT=1",
+ ],
+ "arm": [],
+ }),
+ linkstatic = True,
+ deps = [
+ ":localizer",
+ ":drivetrain_base",
+ "//aos/testing:googletest",
+ "//aos/testing:random_seed",
+ "//aos/testing:test_shm",
+ "//frc971/control_loops/drivetrain:trajectory",
+ "//y2019:constants",
+ "//frc971/control_loops/drivetrain:splinedrivetrain",
+ "@com_github_gflags_gflags//:gflags",
+ ] + cpu_select({
+ "amd64": [
+ "//third_party/matplotlib-cpp",
+ ],
+ "arm": [],
+ }),
+)
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
index ff53b7b..6439d0d 100644
--- a/y2019/control_loops/drivetrain/camera.h
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -193,7 +193,7 @@
::std::vector<::std::vector<Pose>> PlotPoints() const {
::std::vector<::std::vector<Pose>> list_of_lists;
for (const auto &view : target_views()) {
- list_of_lists.push_back({pose_, view.target->pose()});
+ list_of_lists.push_back({pose_, view.target->pose().Rebase(&pose_)});
}
return list_of_lists;
}
@@ -254,9 +254,9 @@
// This number is unitless and if greater than 1, implies that the target is
// visible to the camera and if less than 1 implies it is too small to be
// registered on the camera.
- const Scalar apparent_width =
- ::std::max(0.0, ::std::cos(skew) *
- noise_parameters_.max_viewable_distance / distance);
+ const Scalar apparent_width = ::std::max<Scalar>(
+ 0.0,
+ ::std::cos(skew) * noise_parameters_.max_viewable_distance / distance);
if (apparent_width < 1.0) {
return;
diff --git a/y2019/control_loops/drivetrain/drivetrain_main.cc b/y2019/control_loops/drivetrain/drivetrain_main.cc
index 4e23987..50fd030 100644
--- a/y2019/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2019/control_loops/drivetrain/drivetrain_main.cc
@@ -9,8 +9,11 @@
int main() {
::aos::Init();
::aos::ShmEventLoop event_loop;
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2019::control_loops::drivetrain::GetDrivetrainConfig());
DrivetrainLoop drivetrain(
- ::y2019::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop);
+ ::y2019::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ &localizer);
drivetrain.Run();
::aos::Cleanup();
return 0;
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
new file mode 100644
index 0000000..f011720
--- /dev/null
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -0,0 +1,443 @@
+#ifndef Y2019_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATER_H_
+#define Y2019_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATER_H_
+
+#include <cmath>
+#include <memory>
+
+#include "frc971/control_loops/pose.h"
+#include "y2019/control_loops/drivetrain/camera.h"
+#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+
+namespace y2019 {
+namespace control_loops {
+
+template <int num_cameras, int num_targets, int num_obstacles,
+ int max_targets_per_frame, typename Scalar = double>
+class TypedLocalizer
+ : public ::frc971::control_loops::drivetrain::HybridEkf<Scalar> {
+ public:
+ typedef TypedCamera<num_targets, num_obstacles, Scalar> Camera;
+ typedef typename Camera::TargetView TargetView;
+ typedef typename Camera::Pose Pose;
+ typedef ::frc971::control_loops::drivetrain::HybridEkf<Scalar> HybridEkf;
+ typedef typename HybridEkf::State State;
+ typedef typename HybridEkf::StateSquare StateSquare;
+ typedef typename HybridEkf::Input Input;
+ typedef typename HybridEkf::Output Output;
+ using HybridEkf::kNInputs;
+ using HybridEkf::kNOutputs;
+ using HybridEkf::kNStates;
+
+ // robot_pose should be the object that is used by the cameras, such that when
+ // we update robot_pose, the cameras will change what they report the relative
+ // position of the targets as.
+ // Note that the parameters for the cameras should be set to allow slightly
+ // larger fields of view and slightly longer range than the true cameras so
+ // that we can identify potential matches for targets even when we have slight
+ // modelling errors.
+ TypedLocalizer(
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<Scalar>
+ &dt_config,
+ Pose *robot_pose)
+ : ::frc971::control_loops::drivetrain::HybridEkf<Scalar>(dt_config),
+ robot_pose_(robot_pose) {}
+
+ // Performs a kalman filter correction with a single camera frame, consisting
+ // of up to max_targets_per_frame targets and taken at time t.
+ // camera is the Camera used to take the images.
+ void UpdateTargets(
+ const Camera &camera,
+ const ::aos::SizedArray<TargetView, max_targets_per_frame> &targets,
+ ::aos::monotonic_clock::time_point t) {
+ if (targets.empty()) {
+ return;
+ }
+
+ if (t > HybridEkf::latest_t()) {
+ LOG(ERROR,
+ "target observations must be older than most recent encoder/gyro "
+ "update.");
+ return;
+ }
+
+ Output z;
+ Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
+ TargetViewToMatrices(targets[0], &z, &R);
+
+ // In order to perform the correction steps for the targets, we will
+ // separately perform a Correct step for each following target.
+ // This way, we can have the first correction figure out the mappings
+ // between targets in the image and targets on the field, and then re-use
+ // those mappings for all the remaining corrections.
+ // As such, we need to store the EKF functions that the remaining targets
+ // will need in arrays:
+ ::aos::SizedArray<::std::function<Output(const State &, const Input &)>,
+ max_targets_per_frame> h_functions;
+ ::aos::SizedArray<::std::function<Eigen::Matrix<Scalar, kNOutputs,
+ kNStates>(const State &)>,
+ max_targets_per_frame> dhdx_functions;
+ HybridEkf::Correct(
+ z, nullptr,
+ ::std::bind(&TypedLocalizer::MakeH, this, camera, targets, &h_functions,
+ &dhdx_functions, ::std::placeholders::_1,
+ ::std::placeholders::_2, ::std::placeholders::_3,
+ ::std::placeholders::_4),
+ {}, {}, R, t);
+ // Fetch cache:
+ for (size_t ii = 1; ii < targets.size(); ++ii) {
+ TargetViewToMatrices(targets[ii], &z, &R);
+ HybridEkf::Correct(z, nullptr, {}, h_functions[ii], dhdx_functions[ii], R,
+ t);
+ }
+ }
+
+ private:
+ // The threshold to use for completely rejecting potentially bad target
+ // matches.
+ // TODO(james): Tune
+ static constexpr Scalar kRejectionScore = 1.0;
+
+ // Computes the measurement (z) and noise covariance (R) matrices for a given
+ // TargetView.
+ void TargetViewToMatrices(const TargetView &view, Output *z,
+ Eigen::Matrix<Scalar, kNOutputs, kNOutputs> *R) {
+ *z << view.reading.heading, view.reading.distance, view.reading.skew;
+ // TODO(james): R should account as well for our confidence in the target
+ // matching. However, handling that properly requires thing a lot more about
+ // the probabilities.
+ R->setZero();
+ R->diagonal() << ::std::pow(view.noise.heading, 2),
+ ::std::pow(view.noise.distance, 2), ::std::pow(view.noise.skew, 2);
+ }
+
+ // This is the function that will be called once the Ekf has inserted the
+ // measurement into the right spot in the measurement queue and needs the
+ // output functions to actually perform the corrections.
+ // Specifically, this will take the estimate of the state at that time and
+ // figure out how the targets seen by the camera best map onto the actual
+ // targets on the field.
+ // It then fills in the h and dhdx functions that are called by the Ekf.
+ void MakeH(
+ const Camera &camera,
+ const ::aos::SizedArray<TargetView, max_targets_per_frame> &target_views,
+ ::aos::SizedArray<::std::function<Output(const State &, const Input &)>,
+ max_targets_per_frame> *h_functions,
+ ::aos::SizedArray<::std::function<Eigen::Matrix<Scalar, kNOutputs,
+ kNStates>(const State &)>,
+ max_targets_per_frame> *dhdx_functions,
+ const State &X_hat, const StateSquare &P,
+ ::std::function<Output(const State &, const Input &)> *h,
+ ::std::function<
+ Eigen::Matrix<Scalar, kNOutputs, kNStates>(const State &)> *dhdx) {
+ // Because we need to match camera targets ("views") to actual field
+ // targets, and because we want to take advantage of the correlations
+ // between the targets (i.e., if we see two targets in the image, they
+ // probably correspond to different on-field targets), the matching problem
+ // here is somewhat non-trivial. Some of the methods we use only work
+ // because we are dealing with very small N (e.g., handling the correlations
+ // between multiple views has combinatoric complexity, but since N = 3,
+ // it's not an issue).
+ //
+ // High-level steps:
+ // 1) Set the base robot pose for the cameras to the Pose implied by X_hat.
+ // 2) Fetch all the expected target views from the camera.
+ // 3) Determine the "magnitude" of the Kalman correction from each potential
+ // view/target pair.
+ // 4) Match based on the combination of targets with the smallest
+ // corrections.
+ // 5) Calculate h and dhdx for each pair of targets.
+ //
+ // For the "magnitude" of the correction, we do not directly use the
+ // standard Kalman correction formula. Instead, we calculate the correction
+ // we would get from each component of the measurement and take the L2 norm
+ // of those. This prevents situations where a target matches very poorly but
+ // produces an overall correction of near-zero.
+ // TODO(james): I do not know if this is strictly the correct method to
+ // minimize likely error, but should be reasonable.
+ //
+ // For the matching, we do the following (see MatchFrames):
+ // 1. Compute the best max_targets_per_frame matches for each view.
+ // 2. Exhaust every possible combination of view/target pairs and
+ // choose the best one.
+ // When we don't think the camera should be able to see as many targets as
+ // we actually got in the frame, then we do permit doubling/tripling/etc.
+ // up on potential targets once we've exhausted all the targets we think
+ // we can see.
+
+ // Set the current robot pose so that the cameras know where they are
+ // (all the cameras have robot_pose_ as their base):
+ *robot_pose_->mutable_pos() << X_hat(0, 0), X_hat(1, 0), 0.0;
+ robot_pose_->set_theta(X_hat(2, 0));
+
+ // Compute the things we *think* the camera should be seeing.
+ // Note: Because we will not try to match to any targets that are not
+ // returned by this function, we generally want the modelled camera to have
+ // a slightly larger field of view than the real camera, and be able to see
+ // slightly smaller targets.
+ const ::aos::SizedArray<TargetView, num_targets> camera_views =
+ camera.target_views();
+
+ // Each row contains the scores for each pair of target view and camera
+ // target view. Values in each row will not be populated past
+ // camera.target_views().size(); of the rows, only the first
+ // target_views.size() shall be populated.
+ // Higher scores imply a worse match. Zero implies a perfect match.
+ Eigen::Matrix<Scalar, max_targets_per_frame, num_targets> scores;
+ scores.setConstant(::std::numeric_limits<Scalar>::infinity());
+ // Each row contains the indices of the best matches per view, where
+ // index 0 is the best, 1 the second best, and 2 the third, etc.
+ // -1 indicates an unfilled field.
+ Eigen::Matrix<int, max_targets_per_frame, max_targets_per_frame>
+ best_matches;
+ best_matches.setConstant(-1);
+ // The H matrices for each potential matching. This has the same structure
+ // as the scores matrix.
+ ::std::array<::std::array<Eigen::Matrix<Scalar, kNOutputs, kNStates>,
+ max_targets_per_frame>,
+ num_targets> all_H_matrices;
+
+ // Iterate through and fill out the scores for each potential pairing:
+ for (size_t ii = 0; ii < target_views.size(); ++ii) {
+ const TargetView &target_view = target_views[ii];
+ Output z;
+ Eigen::Matrix<Scalar, kNOutputs, kNOutputs> R;
+ TargetViewToMatrices(target_view, &z, &R);
+
+ for (size_t jj = 0; jj < camera_views.size(); ++jj) {
+ // Compute the ckalman update for this step:
+ const TargetView &view = camera_views[jj];
+ const Eigen::Matrix<Scalar, kNOutputs, kNStates> H =
+ HMatrix(*view.target, target_view);
+ const Eigen::Matrix<Scalar, kNStates, kNOutputs> PH = P * H.transpose();
+ const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> S = H * PH + R;
+ // Note: The inverse here should be very cheap so long as kNOutputs = 3.
+ const Eigen::Matrix<Scalar, kNStates, kNOutputs> K = PH * S.inverse();
+ const Output err = z - Output(view.reading.heading,
+ view.reading.distance, view.reading.skew);
+ // In order to compute the actual score, we want to consider each
+ // component of the error separately, as well as considering the impacts
+ // on the each of the states separately. As such, we calculate what
+ // the separate updates from each error component would be, and sum
+ // the impacts on the states.
+ Output scorer;
+ for (size_t kk = 0; kk < kNOutputs; ++kk) {
+ // TODO(james): squaredNorm or norm or L1-norm? Do we care about the
+ // square root? Do we prefer a quadratic or linear response?
+ scorer(kk, 0) = (K.col(kk) * err(kk, 0)).squaredNorm();
+ }
+ // Compute the overall score--note that we add in a term for the height,
+ // scaled by a manual fudge-factor. The height is not accounted for
+ // in the Kalman update because we are not trying to estimate the height
+ // of the robot directly.
+ Scalar score =
+ scorer.squaredNorm() +
+ ::std::pow((view.reading.height - target_view.reading.height) /
+ target_view.noise.height / 20.0,
+ 2);
+ scores(ii, jj) = score;
+ all_H_matrices[ii][jj] = H;
+
+ // Update the best_matches matrix:
+ int insert_target = jj;
+ for (size_t kk = 0; kk < max_targets_per_frame; ++kk) {
+ int idx = best_matches(ii, kk);
+ // Note that -1 indicates an unfilled value.
+ if (idx == -1 || scores(ii, idx) > scores(ii, insert_target)) {
+ best_matches(ii, kk) = insert_target;
+ insert_target = idx;
+ if (idx == -1) {
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ if (camera_views.size() == 0) {
+ // If we can't get a match, provide H = zero, which will make this
+ // correction step a nop.
+ *h = [](const State &, const Input &) { return Output::Zero(); };
+ *dhdx = [](const State &) {
+ return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero();
+ };
+ for (size_t ii = 0; ii < target_views.size(); ++ii) {
+ h_functions->push_back(*h);
+ dhdx_functions->push_back(*dhdx);
+ }
+ } else {
+ // Go through and brute force the issue of what the best combination of
+ // target matches are. The worst case for this algorithm will be
+ // max_targets_per_frame!, which is awful for any N > ~4, but since
+ // max_targets_per_frame = 3, I'm not really worried.
+ ::std::array<int, max_targets_per_frame> best_frames =
+ MatchFrames(scores, best_matches, target_views.size());
+ for (size_t ii = 0; ii < target_views.size(); ++ii) {
+ int view_idx = best_frames[ii];
+ const Eigen::Matrix<Scalar, kNOutputs, kNStates> best_H =
+ all_H_matrices[ii][view_idx];
+ const TargetView best_view = camera_views[view_idx];
+ const TargetView target_view = target_views[ii];
+ const Scalar match_score = scores(ii, view_idx);
+ if (match_score > kRejectionScore) {
+ h_functions->push_back(
+ [](const State &, const Input &) { return Output::Zero(); });
+ dhdx_functions->push_back([](const State &) {
+ return Eigen::Matrix<Scalar, kNOutputs, kNStates>::Zero();
+ });
+ } else {
+ h_functions->push_back([this, &camera, best_view, target_view](
+ const State &X, const Input &) {
+ // This function actually handles determining what the Output should
+ // be at a given state, now that we have chosen the target that
+ // we want to match to.
+ *robot_pose_->mutable_pos() << X(0, 0), X(1, 0), 0.0;
+ robot_pose_->set_theta(X(2, 0));
+ const Pose relative_pose =
+ best_view.target->pose().Rebase(&camera.pose());
+ const Scalar heading = relative_pose.heading();
+ const Scalar distance = relative_pose.xy_norm();
+ const Scalar skew =
+ ::aos::math::NormalizeAngle(relative_pose.rel_theta());
+ return Output(heading, distance, skew);
+ });
+
+ // TODO(james): Experiment to better understand whether we want to
+ // recalculate H or not.
+ dhdx_functions->push_back(
+ [best_H](const Eigen::Matrix<Scalar, kNStates, 1> &) {
+ return best_H;
+ });
+ }
+ }
+ *h = h_functions->at(0);
+ *dhdx = dhdx_functions->at(0);
+ }
+ }
+
+ Eigen::Matrix<Scalar, kNOutputs, kNStates> HMatrix(
+ const Target &target, const TargetView &target_view) {
+ // To calculate dheading/d{x,y,theta}:
+ // heading = arctan2(target_pos - camera_pos) - camera_theta
+ Eigen::Matrix<Scalar, 3, 1> target_pos = target.pose().abs_pos();
+ Eigen::Matrix<Scalar, 3, 1> camera_pos = target_view.camera_pose.abs_pos();
+ Scalar diffx = target_pos.x() - camera_pos.x();
+ Scalar diffy = target_pos.y() - camera_pos.y();
+ Scalar norm2 = diffx * diffx + diffy * diffy;
+ Scalar dheadingdx = diffy / norm2;
+ Scalar dheadingdy = -diffx / norm2;
+ Scalar dheadingdtheta = -1.0;
+
+ // To calculate ddistance/d{x,y}:
+ // distance = sqrt(diffx^2 + diffy^2)
+ Scalar distance = ::std::sqrt(norm2);
+ Scalar ddistdx = -diffx / distance;
+ Scalar ddistdy = -diffy / distance;
+
+ // Skew = target.theta - camera.theta:
+ Scalar dskewdtheta = -1.0;
+ Eigen::Matrix<Scalar, kNOutputs, kNStates> H;
+ H.setZero();
+ H(0, 0) = dheadingdx;
+ H(0, 1) = dheadingdy;
+ H(0, 2) = dheadingdtheta;
+ H(1, 0) = ddistdx;
+ H(1, 1) = ddistdy;
+ H(2, 2) = dskewdtheta;
+ return H;
+ }
+
+ // A helper function for the fuller version of MatchFrames; this just
+ // removes some of the arguments that are only needed during the recursion.
+ // n_views is the number of targets actually seen in the camera image (i.e.,
+ // the number of rows in scores/best_matches that are actually populated).
+ ::std::array<int, max_targets_per_frame> MatchFrames(
+ const Eigen::Matrix<Scalar, max_targets_per_frame, num_targets> &scores,
+ const Eigen::Matrix<int, max_targets_per_frame, max_targets_per_frame> &
+ best_matches,
+ int n_views) {
+ ::std::array<int, max_targets_per_frame> best_set;
+ Scalar best_score;
+ // We start out without having "used" any views/targets:
+ ::aos::SizedArray<bool, max_targets_per_frame> used_views;
+ for (int ii = 0; ii < n_views; ++ii) {
+ used_views.push_back(false);
+ }
+ MatchFrames(scores, best_matches, used_views, {{false}}, &best_set,
+ &best_score);
+ return best_set;
+ }
+
+ // Recursively iterates over every plausible combination of targets/views
+ // that there is and determines the lowest-scoring combination.
+ // used_views and used_targets indicate which rows/columns of the
+ // scores/best_matches matrices should be ignored. When used_views is all
+ // true, that means that we are done recursing.
+ void MatchFrames(
+ const Eigen::Matrix<Scalar, max_targets_per_frame, num_targets> &scores,
+ const Eigen::Matrix<int, max_targets_per_frame, max_targets_per_frame> &
+ best_matches,
+ const ::aos::SizedArray<bool, max_targets_per_frame> &used_views,
+ const ::std::array<bool, num_targets> &used_targets,
+ ::std::array<int, max_targets_per_frame> *best_set, Scalar *best_score) {
+ *best_score = ::std::numeric_limits<Scalar>::infinity();
+ // Iterate by letting each target in the camera frame (that isn't in
+ // used_views) choose it's best match that isn't already taken. We then set
+ // the appropriate flags in used_views and used_targets and call MatchFrames
+ // to let all the other views sort themselves out.
+ for (size_t ii = 0; ii < used_views.size(); ++ii) {
+ if (used_views[ii]) {
+ continue;
+ }
+ int best_match = -1;
+ for (size_t jj = 0; jj < max_targets_per_frame; ++jj) {
+ if (best_matches(ii, jj) == -1) {
+ // If we run out of potential targets from the camera, then there
+ // are more targets in the frame than we think there should be.
+ // In this case, we are going to be doubling/tripling/etc. up
+ // anyhow. So we just give everyone their top choice:
+ // TODO(james): If we ever are dealing with larger numbers of
+ // targets per frame, do something to minimize doubling-up.
+ best_match = best_matches(ii, 0);
+ break;
+ }
+ best_match = best_matches(ii, jj);
+ if (!used_targets[best_match]) {
+ break;
+ }
+ }
+ // If we reach here and best_match = -1, that means that no potential
+ // targets were generated by the camera, and we should never have gotten
+ // here.
+ CHECK(best_match != -1);
+ ::aos::SizedArray<bool, max_targets_per_frame> sub_views = used_views;
+ sub_views[ii] = true;
+ ::std::array<bool, num_targets> sub_targets = used_targets;
+ sub_targets[best_match] = true;
+ ::std::array<int, max_targets_per_frame> sub_best_set;
+ Scalar score;
+ MatchFrames(scores, best_matches, sub_views, sub_targets, &sub_best_set,
+ &score);
+ score += scores(ii, best_match);
+ sub_best_set[ii] = best_match;
+ if (score < *best_score) {
+ *best_score = score;
+ *best_set = sub_best_set;
+ }
+ }
+ // best_score will be infinite if we did not find a result due to there
+ // being no targets that weren't set in used_vies; this is the
+ // base case of the recursion and so we set best_score to zero:
+ if (!::std::isfinite(*best_score)) {
+ *best_score = 0.0;
+ }
+ }
+
+ // The pose that is used by the cameras to determine the location of the robot
+ // and thus the expected view of the targets.
+ Pose *robot_pose_;
+}; // class TypedLocalizer
+
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATER_H_
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
new file mode 100644
index 0000000..45ae714
--- /dev/null
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -0,0 +1,603 @@
+#include "y2019/control_loops/drivetrain/localizer.h"
+
+#include <random>
+#include <queue>
+
+#include "aos/testing/random_seed.h"
+#include "aos/testing/test_shm.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
+#include "gflags/gflags.h"
+#if defined(SUPPORT_PLOT)
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#endif
+#include "gtest/gtest.h"
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
+#include "y2019/constants.h"
+
+DEFINE_bool(plot, false, "If true, plot");
+
+namespace y2019 {
+namespace control_loops {
+namespace testing {
+
+using ::y2019::constants::Field;
+
+constexpr size_t kNumCameras = 4;
+constexpr size_t kNumTargetsPerFrame = 3;
+
+typedef TypedLocalizer<kNumCameras, Field::kNumTargets, Field::kNumObstacles,
+ kNumTargetsPerFrame, double> TestLocalizer;
+typedef typename TestLocalizer::Camera TestCamera;
+typedef typename TestCamera::Pose Pose;
+typedef typename TestCamera::LineSegment Obstacle;
+
+typedef TestLocalizer::StateIdx StateIdx;
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+
+// When placing the cameras on the robot, set them all kCameraOffset out from
+// the center, to test that we really can handle cameras that aren't at the
+// center-of-mass.
+constexpr double kCameraOffset = 0.1;
+
+#if defined(SUPPORT_PLOT)
+// Plots a line from a vector of Pose's.
+void PlotPlotPts(const ::std::vector<Pose> &poses,
+ const ::std::map<::std::string, ::std::string> &kwargs) {
+ ::std::vector<double> x;
+ ::std::vector<double> y;
+ for (const Pose & p : poses) {
+ x.push_back(p.abs_pos().x());
+ y.push_back(p.abs_pos().y());
+ }
+ matplotlibcpp::plot(x, y, kwargs);
+}
+#endif
+
+struct LocalizerTestParams {
+ // Control points for the spline to make the robot follow.
+ ::std::array<float, 6> control_pts_x;
+ ::std::array<float, 6> control_pts_y;
+ // The actual state to start the robot at. By setting voltage errors and the
+ // such you can introduce persistent disturbances.
+ TestLocalizer::State true_start_state;
+ // The initial state of the estimator.
+ TestLocalizer::State known_start_state;
+ // Whether or not to add Gaussian noise to the sensors and cameras.
+ bool noisify;
+ // Whether or not to add unmodelled disturbances.
+ bool disturb;
+ // The tolerances for the estimator and for the trajectory following at
+ // the end of the spline:
+ double estimate_tolerance;
+ double goal_tolerance;
+};
+
+class ParameterizedLocalizerTest
+ : public ::testing::TestWithParam<LocalizerTestParams> {
+ public:
+ ::aos::testing::TestSharedMemory shm_;
+
+ // Set up three targets in a row, at (-1, 0), (0, 0), and (1, 0).
+ // Make the right-most target (1, 0) be facing away from the camera, and give
+ // the middle target some skew.
+ // Place one camera facing forward, the other facing backward, and set the
+ // robot at (0, -5) with the cameras each 0.1m from the center.
+ // Place one obstacle in a place where it can block the left-most target (-1,
+ // 0).
+ ParameterizedLocalizerTest()
+ : field_(),
+ targets_(field_.targets()),
+ modeled_obstacles_(field_.obstacles()),
+ true_obstacles_(field_.obstacles()),
+ dt_config_(drivetrain::GetDrivetrainConfig()),
+ // Pull the noise for the encoders/gyros from the R matrix:
+ encoder_noise_(::std::sqrt(
+ dt_config_.make_kf_drivetrain_loop().observer().coefficients().R(
+ 0, 0))),
+ gyro_noise_(::std::sqrt(
+ dt_config_.make_kf_drivetrain_loop().observer().coefficients().R(
+ 2, 2))),
+ // As per the comments in localizer.h, we set the field of view and
+ // noise parameters on the robot_cameras_ so that they see a bit more
+ // than the true_cameras_. The robot_cameras_ are what is passed to the
+ // localizer and used to generate "expected" targets. The true_cameras_
+ // are what we actually use to generate targets to pass to the
+ // localizer.
+ robot_cameras_{
+ {TestCamera({&robot_pose_, {0.0, kCameraOffset, 0.0}, M_PI_2},
+ M_PI_2 * 1.1, robot_noise_parameters_, targets_,
+ modeled_obstacles_),
+ TestCamera({&robot_pose_, {kCameraOffset, 0.0, 0.0}, 0.0},
+ M_PI_2 * 1.1, robot_noise_parameters_, targets_,
+ modeled_obstacles_),
+ TestCamera({&robot_pose_, {-kCameraOffset, 0.0, 0.0}, M_PI},
+ M_PI_2 * 1.1, robot_noise_parameters_, targets_,
+ modeled_obstacles_),
+ TestCamera({&robot_pose_, {0.0, -kCameraOffset, 0.0}, -M_PI_2},
+ M_PI_2 * 1.1, robot_noise_parameters_, targets_,
+ modeled_obstacles_)}},
+ true_cameras_{
+ {TestCamera({&true_robot_pose_, {0.0, kCameraOffset, 0.0}, M_PI_2},
+ M_PI_2 * 0.9, true_noise_parameters_, targets_,
+ true_obstacles_),
+ TestCamera({&true_robot_pose_, {kCameraOffset, 0.0, 0.0}, 0.0},
+ M_PI_2 * 0.9, true_noise_parameters_, targets_,
+ true_obstacles_),
+ TestCamera({&true_robot_pose_, {-kCameraOffset, 0.0, 0.0}, M_PI},
+ M_PI_2 * 0.9, true_noise_parameters_, targets_,
+ true_obstacles_),
+ TestCamera(
+ {&true_robot_pose_, {-0.0, -kCameraOffset, 0.0}, -M_PI_2},
+ M_PI_2 * 0.9, true_noise_parameters_, targets_,
+ true_obstacles_)}},
+ localizer_(dt_config_, &robot_pose_),
+ spline_drivetrain_(dt_config_) {
+ // We use the default P() for initialization.
+ localizer_.ResetInitialState(t0_, GetParam().known_start_state,
+ localizer_.P());
+ }
+
+ void SetUp() {
+ ::frc971::control_loops::DrivetrainQueue::Goal goal;
+ goal.controller_type = 2;
+ goal.spline.spline_idx = 1;
+ goal.spline.spline_count = 1;
+ goal.spline_handle = 1;
+ ::std::copy(GetParam().control_pts_x.begin(),
+ GetParam().control_pts_x.end(), goal.spline.spline_x.begin());
+ ::std::copy(GetParam().control_pts_y.begin(),
+ GetParam().control_pts_y.end(), goal.spline.spline_y.begin());
+ spline_drivetrain_.SetGoal(goal);
+ }
+
+ void TearDown() {
+ printf("Each iteration of the simulation took on average %f seconds.\n",
+ avg_time_.count());
+#if defined(SUPPORT_PLOT)
+ if (FLAGS_plot) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(simulation_t_, simulation_vl_, {{"label", "Vl"}});
+ matplotlibcpp::plot(simulation_t_, simulation_vr_, {{"label", "Vr"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(spline_x_, spline_y_, {{"label", "spline"}});
+ matplotlibcpp::plot(simulation_x_, simulation_y_, {{"label", "robot"}});
+ matplotlibcpp::plot(estimated_x_, estimated_y_,
+ {{"label", "estimation"}});
+ for (const Target & target : targets_) {
+ PlotPlotPts(target.PlotPoints(), {{"c", "g"}});
+ }
+ for (const Obstacle &obstacle : true_obstacles_) {
+ PlotPlotPts(obstacle.PlotPoints(), {{"c", "k"}});
+ }
+ // Go through and plot true/expected camera targets for a few select
+ // time-steps.
+ ::std::vector<const char *> colors{"m", "y", "c"};
+ int jj = 0;
+ for (size_t ii = 0; ii < simulation_x_.size(); ii += 400) {
+ *true_robot_pose_.mutable_pos() << simulation_x_[ii], simulation_y_[ii],
+ 0.0;
+ true_robot_pose_.set_theta(simulation_theta_[ii]);
+ for (const TestCamera &camera : true_cameras_) {
+ for (const auto &plot_pts : camera.PlotPoints()) {
+ PlotPlotPts(plot_pts, {{"c", colors[jj]}});
+ }
+ }
+ for (const TestCamera &camera : robot_cameras_) {
+ *robot_pose_.mutable_pos() << estimated_x_[ii], estimated_y_[ii], 0.0;
+ robot_pose_.set_theta(estimated_theta_[ii]);
+ const auto &all_plot_pts = camera.PlotPoints();
+ *robot_pose_.mutable_pos() = true_robot_pose_.rel_pos();
+ robot_pose_.set_theta(true_robot_pose_.rel_theta());
+ for (const auto &plot_pts : all_plot_pts) {
+ PlotPlotPts(plot_pts, {{"c", colors[jj]}, {"ls", "--"}});
+ }
+ }
+ jj = (jj + 1) % colors.size();
+ }
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(
+ simulation_t_, spline_x_,
+ {{"label", "spline x"}, {"c", "g"}, {"ls", ""}, {"marker", "o"}});
+ matplotlibcpp::plot(simulation_t_, simulation_x_,
+ {{"label", "simulated x"}, {"c", "g"}});
+ matplotlibcpp::plot(simulation_t_, estimated_x_,
+ {{"label", "estimated x"}, {"c", "g"}, {"ls", "--"}});
+
+ matplotlibcpp::plot(
+ simulation_t_, spline_y_,
+ {{"label", "spline y"}, {"c", "b"}, {"ls", ""}, {"marker", "o"}});
+ matplotlibcpp::plot(simulation_t_, simulation_y_,
+ {{"label", "simulated y"}, {"c", "b"}});
+ matplotlibcpp::plot(simulation_t_, estimated_y_,
+ {{"label", "estimated y"}, {"c", "b"}, {"ls", "--"}});
+
+ matplotlibcpp::plot(simulation_t_, simulation_theta_,
+ {{"label", "simulated theta"}, {"c", "r"}});
+ matplotlibcpp::plot(
+ simulation_t_, estimated_theta_,
+ {{"label", "estimated theta"}, {"c", "r"}, {"ls", "--"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::show();
+ }
+#endif
+ }
+
+ protected:
+ // Returns a random number with a gaussian distribution with a mean of zero
+ // and a standard deviation of std, if noisify = true.
+ // If noisify is false, then returns 0.0.
+ double Normal(double std) {
+ if (GetParam().noisify) {
+ return normal_(gen_) * std;
+ }
+ return 0.0;
+ }
+ // Adds random noise to the given target view.
+ void Noisify(TestCamera::TargetView *view) {
+ view->reading.heading += Normal(view->noise.heading);
+ view->reading.distance += Normal(view->noise.distance);
+ view->reading.height += Normal(view->noise.height);
+ view->reading.skew += Normal(view->noise.skew);
+ }
+ // The differential equation for the dynamics of the system.
+ TestLocalizer::State DiffEq(const TestLocalizer::State &X,
+ const TestLocalizer::Input &U) {
+ return localizer_.DiffEq(X, U);
+ }
+
+ Field field_;
+ ::std::array<Target, Field::kNumTargets> targets_;
+ // The obstacles that are passed to the camera objects for the localizer.
+ ::std::array<Obstacle, Field::kNumObstacles> modeled_obstacles_;
+ // The obstacles that are used for actually simulating the cameras.
+ ::std::array<Obstacle, Field::kNumObstacles> true_obstacles_;
+
+ DrivetrainConfig<double> dt_config_;
+
+ // Noise information for the actual simulated cameras (true_*) and the
+ // parameters that the localizer should use for modelling the cameras. Note
+ // how the max_viewable_distance is larger for the localizer, so that if
+ // there is any error in the estimation, it still thinks that it can see
+ // any targets that might actually be in range.
+ TestCamera::NoiseParameters true_noise_parameters_ = {
+ .max_viewable_distance = 10.0,
+ .heading_noise = 0.02,
+ .nominal_distance_noise = 0.06,
+ .nominal_skew_noise = 0.1,
+ .nominal_height_noise = 0.01};
+ TestCamera::NoiseParameters robot_noise_parameters_ = {
+ .max_viewable_distance = 11.0,
+ .heading_noise = 0.02,
+ .nominal_distance_noise = 0.06,
+ .nominal_skew_noise = 0.1,
+ .nominal_height_noise = 0.01};
+
+ // Standard deviations of the noise for the encoders/gyro.
+ double encoder_noise_, gyro_noise_;
+
+ Pose robot_pose_;
+ ::std::array<TestCamera, 4> robot_cameras_;
+ Pose true_robot_pose_;
+ ::std::array<TestCamera, 4> true_cameras_;
+
+ TestLocalizer localizer_;
+
+ ::frc971::control_loops::drivetrain::SplineDrivetrain spline_drivetrain_;
+
+ // All the data we want to end up plotting.
+ ::std::vector<double> simulation_t_;
+
+ ::std::vector<double> spline_x_;
+ ::std::vector<double> spline_y_;
+ ::std::vector<double> estimated_x_;
+ ::std::vector<double> estimated_y_;
+ ::std::vector<double> estimated_theta_;
+ ::std::vector<double> simulation_x_;
+ ::std::vector<double> simulation_y_;
+ ::std::vector<double> simulation_theta_;
+
+ ::std::vector<double> simulation_vl_;
+ ::std::vector<double> simulation_vr_;
+
+ // Simulation start time
+ ::aos::monotonic_clock::time_point t0_;
+
+ // Average duration of each iteration; used for debugging and getting a
+ // sanity-check on what performance looks like--uses a real system clock.
+ ::std::chrono::duration<double> avg_time_;
+
+ ::std::mt19937 gen_{static_cast<uint32_t>(::aos::testing::RandomSeed())};
+ ::std::normal_distribution<> normal_;
+};
+
+// Tests that, when we attempt to follow a spline and use the localizer to
+// perform the state estimation, we end up roughly where we should and that
+// the localizer has a valid position estimate.
+TEST_P(ParameterizedLocalizerTest, SplineTest) {
+ // state stores the true state of the robot throughout the simulation.
+ TestLocalizer::State state = GetParam().true_start_state;
+
+ ::aos::monotonic_clock::time_point t = t0_;
+
+ // The period with which we should take frames from the cameras. Currently,
+ // we just trigger all the cameras at once, rather than offsetting them or
+ // anything.
+ const int camera_period = 5; // cycles
+ // The amount of time to delay the camera images from when they are taken.
+ const ::std::chrono::milliseconds camera_latency(50);
+
+ // A queue of camera frames so that we can add a time delay to the data
+ // coming from the cameras.
+ ::std::queue<::std::tuple<
+ ::aos::monotonic_clock::time_point, const TestCamera *,
+ ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame>>>
+ camera_queue;
+
+ // Start time, for debugging.
+ const auto begin = ::std::chrono::steady_clock::now();
+
+ size_t i;
+ for (i = 0; !spline_drivetrain_.IsAtEnd(); ++i) {
+ // Get the current state estimate into a matrix that works for the
+ // trajectory code.
+ ::Eigen::Matrix<double, 5, 1> known_state;
+ TestLocalizer::State X_hat = localizer_.X_hat();
+ known_state << X_hat(StateIdx::kX, 0), X_hat(StateIdx::kY, 0),
+ X_hat(StateIdx::kTheta, 0), X_hat(StateIdx::kLeftVelocity, 0),
+ X_hat(StateIdx::kRightVelocity, 0);
+
+ spline_drivetrain_.Update(true, known_state);
+
+ ::frc971::control_loops::DrivetrainQueue::Output output;
+ output.left_voltage = 0;
+ output.right_voltage = 0;
+ spline_drivetrain_.SetOutput(&output);
+ TestLocalizer::Input U(output.left_voltage, output.right_voltage);
+
+ const ::Eigen::Matrix<double, 5, 1> goal_state =
+ spline_drivetrain_.CurrentGoalState();
+ simulation_t_.push_back(
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ t.time_since_epoch())
+ .count());
+ spline_x_.push_back(goal_state(0, 0));
+ spline_y_.push_back(goal_state(1, 0));
+ simulation_x_.push_back(state(StateIdx::kX, 0));
+ simulation_y_.push_back(state(StateIdx::kY, 0));
+ simulation_theta_.push_back(state(StateIdx::kTheta, 0));
+ estimated_x_.push_back(known_state(0, 0));
+ estimated_y_.push_back(known_state(1, 0));
+ estimated_theta_.push_back(known_state(StateIdx::kTheta, 0));
+
+ simulation_vl_.push_back(U(0));
+ simulation_vr_.push_back(U(1));
+ U(0, 0) = ::std::max(::std::min(U(0, 0), 12.0), -12.0);
+ U(1, 0) = ::std::max(::std::min(U(1, 0), 12.0), -12.0);
+
+ state = ::frc971::control_loops::RungeKuttaU(
+ [this](const ::Eigen::Matrix<double, 10, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) { return DiffEq(X, U); },
+ state, U,
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ dt_config_.dt)
+ .count());
+
+ // Add arbitrary disturbances at some arbitrary interval. The main points of
+ // interest here are that we (a) stop adding disturbances at the very end of
+ // the trajectory, to allow us to actually converge to the goal, and (b)
+ // scale disturbances by the corruent velocity.
+ if (GetParam().disturb && i % 50 == 0) {
+ // Scale the disturbance so that when we have near-zero velocity we don't
+ // have much disturbance.
+ double disturbance_scale = ::std::min(
+ 1.0, ::std::sqrt(::std::pow(state(StateIdx::kLeftVelocity, 0), 2) +
+ ::std::pow(state(StateIdx::kRightVelocity, 0), 2)) /
+ 3.0);
+ TestLocalizer::State disturbance;
+ disturbance << 0.02, 0.02, 0.001, 0.03, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0;
+ disturbance *= disturbance_scale;
+ state += disturbance;
+ }
+
+ t += dt_config_.dt;
+ *true_robot_pose_.mutable_pos() << state(StateIdx::kX, 0),
+ state(StateIdx::kY, 0), 0.0;
+ true_robot_pose_.set_theta(state(StateIdx::kTheta, 0));
+ const double left_enc = state(StateIdx::kLeftEncoder, 0);
+ const double right_enc = state(StateIdx::kRightEncoder, 0);
+
+ const double gyro = (state(StateIdx::kRightVelocity, 0) -
+ state(StateIdx::kLeftVelocity, 0)) /
+ dt_config_.robot_radius / 2.0;
+
+ localizer_.UpdateEncodersAndGyro(left_enc + Normal(encoder_noise_),
+ right_enc + Normal(encoder_noise_),
+ gyro + Normal(gyro_noise_), U, t);
+
+ // Clear out the camera frames that we are ready to pass to the localizer.
+ while (!camera_queue.empty() &&
+ ::std::get<0>(camera_queue.front()) < t - camera_latency) {
+ const auto tuple = camera_queue.front();
+ camera_queue.pop();
+ ::aos::monotonic_clock::time_point t_obs = ::std::get<0>(tuple);
+ const TestCamera *camera = ::std::get<1>(tuple);
+ ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame> views =
+ ::std::get<2>(tuple);
+ localizer_.UpdateTargets(*camera, views, t_obs);
+ }
+
+ // Actually take all the images and store them in the queue.
+ if (i % camera_period == 0) {
+ for (size_t jj = 0; jj < true_cameras_.size(); ++jj) {
+ const auto target_views = true_cameras_[jj].target_views();
+ ::aos::SizedArray<TestCamera::TargetView, kNumTargetsPerFrame>
+ pass_views;
+ for (size_t ii = 0;
+ ii < ::std::min(kNumTargetsPerFrame, target_views.size()); ++ii) {
+ TestCamera::TargetView view = target_views[ii];
+ Noisify(&view);
+ pass_views.push_back(view);
+ }
+ camera_queue.emplace(t, &robot_cameras_[jj], pass_views);
+ }
+ }
+
+ }
+
+ const auto end = ::std::chrono::steady_clock::now();
+ avg_time_ = (end - begin) / i;
+ TestLocalizer::State estimate_err = state - localizer_.X_hat();
+ EXPECT_LT(estimate_err.template topRows<7>().norm(),
+ GetParam().estimate_tolerance);
+ // Check that none of the states that we actually care about (x/y/theta, and
+ // wheel positions/speeds) are too far off, individually:
+ EXPECT_LT(estimate_err.template topRows<7>().cwiseAbs().maxCoeff(),
+ GetParam().estimate_tolerance / 3.0)
+ << "Estimate error: " << estimate_err.transpose();
+ Eigen::Matrix<double, 5, 1> final_trajectory_state;
+ final_trajectory_state << state(StateIdx::kX, 0), state(StateIdx::kY, 0),
+ state(StateIdx::kTheta, 0), state(StateIdx::kLeftVelocity, 0),
+ state(StateIdx::kRightVelocity, 0);
+ const Eigen::Matrix<double, 5, 1> final_trajectory_state_err =
+ final_trajectory_state - spline_drivetrain_.CurrentGoalState();
+ EXPECT_LT(final_trajectory_state_err.norm(), GetParam().goal_tolerance)
+ << "Goal error: " << final_trajectory_state_err.transpose();
+}
+
+INSTANTIATE_TEST_CASE_P(
+ LocalizerTest, ParameterizedLocalizerTest,
+ ::testing::Values(
+ // Checks a "perfect" scenario, where we should track perfectly.
+ LocalizerTestParams({
+ /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
+ /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ /*noisify=*/false,
+ /*disturb=*/false,
+ /*estimate_tolerance=*/1e-5,
+ /*goal_tolerance=*/2e-2,
+ }),
+ // Checks "perfect" estimation, but start off the spline and check
+ // that we can still follow it.
+ LocalizerTestParams({
+ /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
+ /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
+ (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ (TestLocalizer::State() << 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ /*noisify=*/false,
+ /*disturb=*/false,
+ /*estimate_tolerance=*/1e-5,
+ /*goal_tolerance=*/2e-2,
+ }),
+ // Repeats perfect scenario, but add sensor noise.
+ LocalizerTestParams({
+ /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
+ /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ /*noisify=*/true,
+ /*disturb=*/false,
+ /*estimate_tolerance=*/0.2,
+ /*goal_tolerance=*/0.2,
+ }),
+ // Repeats perfect scenario, but add initial estimator error.
+ LocalizerTestParams({
+ /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
+ /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0)
+ .finished(),
+ /*noisify=*/false,
+ /*disturb=*/false,
+ /*estimate_tolerance=*/1e-4,
+ /*goal_tolerance=*/2e-2,
+ }),
+ // Repeats perfect scenario, but add voltage + angular errors:
+ LocalizerTestParams({
+ /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
+ /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,
+ 0.5, 0.02)
+ .finished(),
+ (TestLocalizer::State() << 0.1, -5.1, -0.01, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0)
+ .finished(),
+ /*noisify=*/false,
+ /*disturb=*/false,
+ /*estimate_tolerance=*/1e-4,
+ /*goal_tolerance=*/2e-2,
+ }),
+ // Add disturbances while we are driving:
+ LocalizerTestParams({
+ /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
+ /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ /*noisify=*/false,
+ /*disturb=*/true,
+ /*estimate_tolerance=*/2e-2,
+ /*goal_tolerance=*/0.15,
+ }),
+ // Add noise and some initial error in addition:
+ LocalizerTestParams({
+ /*control_pts_x=*/{{0.0, 3.0, 3.0, 0.0, 1.0, 1.0}},
+ /*control_pts_y=*/{{-5.0, -5.0, 2.0, 2.0, 2.0, 3.0}},
+ (TestLocalizer::State() << 0.0, -5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ (TestLocalizer::State() << 0.1, -5.1, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ /*noisify=*/true,
+ /*disturb=*/true,
+ /*estimate_tolerance=*/0.15,
+ /*goal_tolerance=*/0.5,
+ }),
+ // Try another spline, just in case the one I was using is special for
+ // some reason; this path will also go straight up to a target, to
+ // better simulate what might happen when trying to score:
+ LocalizerTestParams({
+ /*control_pts_x=*/{{0.5, 3.5, 4.0, 8.0, 11.0, 10.2}},
+ /*control_pts_y=*/{{1.0, 1.0, -3.0, -2.0, -3.5, -3.65}},
+ (TestLocalizer::State() << 0.6, 1.01, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ (TestLocalizer::State() << 0.5, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0)
+ .finished(),
+ /*noisify=*/true,
+ /*disturb=*/false,
+ /*estimate_tolerance=*/0.1,
+ /*goal_tolerance=*/0.5,
+ })));
+
+} // namespace testing
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index f55e1ad..00bacae 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -24,6 +24,7 @@
],
deps = [
":collision_avoidance",
+ ":vacuum",
":superstructure_queue",
"//aos/controls:control_loop",
"//y2019:constants",
@@ -77,6 +78,20 @@
],
)
+cc_library(
+ name = "vacuum",
+ srcs = [
+ "vacuum.cc",
+ ],
+ hdrs = [
+ "vacuum.h",
+ ],
+ deps = [
+ ":superstructure_queue",
+ "//aos/controls:control_loop"
+ ],
+)
+
cc_test(
name = "collision_avoidance_tests",
srcs = [
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 187379b..3443d63 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -8,35 +8,6 @@
namespace control_loops {
namespace superstructure {
-void Superstructure::HandleSuction(const SuctionGoal *unsafe_goal,
- float suction_pressure,
- SuperstructureQueue::Output *output,
- bool *has_piece) {
- constexpr double kPumpVoltage = 12.0;
- constexpr double kPumpHasPieceVoltage = 8.0;
-
- // TODO(austin): Low pass filter on pressure.
- *has_piece = suction_pressure < 0.70;
-
- if (unsafe_goal && output) {
- const bool evacuate = unsafe_goal->top || unsafe_goal->bottom;
- if (evacuate) {
- vacuum_count_ = 200;
- }
- // TODO(austin): High speed pump a bit longer after we detect we have the
- // game piece.
- // Once the vacuum evacuates, the pump speeds up because there is no
- // resistance. So, we want to turn it down to save the pump from
- // overheating.
- output->pump_voltage =
- (vacuum_count_ > 0) ? (*has_piece ? kPumpHasPieceVoltage : kPumpVoltage)
- : 0.0;
- output->intake_suction_top = unsafe_goal->top;
- output->intake_suction_bottom = unsafe_goal->bottom;
- }
- vacuum_count_ = ::std::max(0, vacuum_count_ - 1);
-}
-
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
@@ -77,8 +48,9 @@
output != nullptr ? &(output->stilts_voltage) : nullptr,
&(status->stilts));
- HandleSuction(unsafe_goal != nullptr ? &(unsafe_goal->suction) : nullptr,
- position->suction_pressure, output, &(status->has_piece));
+ vacuum_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->suction) : nullptr,
+ position->suction_pressure, output, &(status->has_piece),
+ event_loop());
status->zeroed = status->elevator.zeroed && status->wrist.zeroed &&
status->intake.zeroed && status->stilts.zeroed;
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 9879e17..626c84b 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -6,6 +6,7 @@
#include "y2019/constants.h"
#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/superstructure/vacuum.h"
namespace y2019 {
namespace control_loops {
@@ -32,6 +33,7 @@
const PotAndAbsoluteEncoderSubsystem &wrist() const { return wrist_; }
const AbsoluteEncoderSubsystem &intake() const { return intake_; }
const PotAndAbsoluteEncoderSubsystem &stilts() const { return stilts_; }
+ const Vacuum &vacuum() const { return vacuum_; }
protected:
virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
@@ -40,16 +42,13 @@
SuperstructureQueue::Status *status) override;
private:
- void HandleSuction(const SuctionGoal *unsafe_goal, float suction_pressure,
- SuperstructureQueue::Output *output, bool *has_piece);
-
PotAndAbsoluteEncoderSubsystem elevator_;
PotAndAbsoluteEncoderSubsystem wrist_;
AbsoluteEncoderSubsystem intake_;
PotAndAbsoluteEncoderSubsystem stilts_;
+ Vacuum vacuum_;
CollisionAvoidance collision_avoidance_;
- int vacuum_count_ = 0;
static constexpr double kMinIntakeAngleForRollers = -0.7;
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index 4176057..f87e4f2 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -25,8 +25,8 @@
.frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal wrist;
// Distance stilts extended out of the bottom of the robot. Positive = down.
- // 0 is the height such that the bottom of the stilts is tangent to the bottom
- // of the middle wheels.
+ // 0 is the height such that the bottom of the stilts is tangent to the
+ // bottom of the middle wheels.
.frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal stilts;
// Positive is rollers intaking inward.
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index bd18b60..4efb95f 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -128,6 +128,8 @@
wrist_pot_encoder_.GetSensorValues(&position->wrist);
intake_pot_encoder_.GetSensorValues(&position->intake_joint);
stilts_pot_encoder_.GetSensorValues(&position->stilts);
+ position->suction_pressure = simulated_pressure_;
+
position.Send();
}
@@ -162,6 +164,10 @@
stilts_plant_->set_voltage_offset(voltage_offset);
}
+ void set_simulated_pressure(double pressure) {
+ simulated_pressure_ = pressure;
+ }
+
// Simulates the superstructure for a single timestep.
void Simulate() {
EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
@@ -266,6 +272,8 @@
::std::unique_ptr<CappedTestPlant> stilts_plant_;
PositionSensorSimulator stilts_pot_encoder_;
+ double simulated_pressure_ = 1.0;
+
SuperstructureQueue superstructure_queue_;
};
@@ -305,7 +313,7 @@
superstructure_.Iterate();
superstructure_plant_.Simulate();
- TickTime(::std::chrono::microseconds(5050));
+ TickTime(chrono::microseconds(5050));
}
void CheckCollisions() {
@@ -698,6 +706,98 @@
VerifyNearGoal();
}
+// Tests the Vacuum detects a gamepiece
+TEST_F(SuperstructureTest, VacuumDetectsPiece) {
+ WaitUntilZeroed();
+ // Turn on suction
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->suction.top = true;
+ goal->suction.bottom = true;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(
+ Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10),
+ true, false);
+
+ // Verify that at 0 pressure after short time voltage is still 12
+ superstructure_plant_.set_simulated_pressure(0.0);
+ RunForTime(chrono::seconds(2));
+ superstructure_queue_.status.FetchLatest();
+ EXPECT_TRUE(superstructure_queue_.status->has_piece);
+}
+
+// Tests the Vacuum backs off after acquiring a gamepiece
+TEST_F(SuperstructureTest, VacuumBacksOff) {
+ WaitUntilZeroed();
+ // Turn on suction
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->suction.top = true;
+ goal->suction.bottom = true;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ // Verify that at 0 pressure after short time voltage is still high
+ superstructure_plant_.set_simulated_pressure(0.0);
+ RunForTime(
+ Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10),
+ true, false);
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->pump_voltage, Vacuum::kPumpVoltage);
+
+ // Verify that after waiting with a piece the pump voltage goes to the
+ // has piece voltage
+ RunForTime(chrono::seconds(2), true, false);
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->pump_voltage,
+ Vacuum::kPumpHasPieceVoltage);
+}
+
+// Tests the Vacuum stays on for a bit after getting a no suck goal
+TEST_F(SuperstructureTest, VacuumStaysOn) {
+ WaitUntilZeroed();
+ // Turn on suction
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->suction.top = true;
+ goal->suction.bottom = true;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ // Get a Gamepiece
+ superstructure_plant_.set_simulated_pressure(0.0);
+ RunForTime(chrono::seconds(2));
+ superstructure_queue_.status.FetchLatest();
+ EXPECT_TRUE(superstructure_queue_.status->has_piece);
+
+ // Turn off suction
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->suction.top = false;
+ goal->suction.bottom = false;
+ ASSERT_TRUE(goal.Send());
+ }
+
+ superstructure_plant_.set_simulated_pressure(1.0);
+ // Run for a short while and make sure we still ask for non-zero volts
+ RunForTime(Vacuum::kTimeToKeepPumpRunning -
+ chrono::milliseconds(10),
+ true, false);
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_GE(superstructure_queue_.output->pump_voltage,
+ Vacuum::kPumpHasPieceVoltage);
+
+ // Wait and make sure the vacuum actually turns off
+ RunForTime(chrono::seconds(2));
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->pump_voltage, 0.0);
+}
+
// Tests that running disabled, ya know, works
TEST_F(SuperstructureTest, DiasableTest) {
RunForTime(chrono::seconds(2), false, false);
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
new file mode 100644
index 0000000..21acb2e
--- /dev/null
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -0,0 +1,57 @@
+#include "y2019/control_loops/superstructure/vacuum.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+constexpr double Vacuum::kPumpVoltage;
+constexpr double Vacuum::kPumpHasPieceVoltage;
+constexpr aos::monotonic_clock::duration Vacuum::kTimeAtHigherVoltage;
+constexpr aos::monotonic_clock::duration Vacuum::kTimeToKeepPumpRunning;
+
+void Vacuum::Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
+ SuperstructureQueue::Output *output, bool *has_piece,
+ aos::EventLoop *event_loop) {
+ auto monotonic_now = event_loop->monotonic_now();
+ bool low_pump_voltage = false;
+ bool no_goal_for_a_bit = false;
+
+ // implement a simple low-pass filter on the pressure
+ filtered_pressure_ = kSuctionAlpha * suction_pressure +
+ (1 - kSuctionAlpha) * filtered_pressure_;
+
+ *has_piece = filtered_pressure_ < kVacuumThreshold;
+
+ if (*has_piece && !had_piece_) {
+ time_at_last_acquisition_ = monotonic_now;
+ }
+
+ // if we've had the piece for enought time, go to lower pump_voltage
+ low_pump_voltage =
+ *has_piece &&
+ monotonic_now > time_at_last_acquisition_ + kTimeAtHigherVoltage;
+ no_goal_for_a_bit =
+ monotonic_now > time_at_last_evacuate_goal_ + kTimeToKeepPumpRunning;
+
+ if (unsafe_goal && output) {
+ const bool evacuate = unsafe_goal->top || unsafe_goal->bottom;
+ if (evacuate) {
+ time_at_last_evacuate_goal_ = monotonic_now;
+ }
+
+ // Once the vacuum evacuates, the pump speeds up because there is no
+ // resistance. So, we want to turn it down to save the pump from
+ // overheating.
+ output->pump_voltage =
+ (no_goal_for_a_bit) ? 0 : (low_pump_voltage ? kPumpHasPieceVoltage
+ : kPumpVoltage);
+
+ output->intake_suction_top = unsafe_goal->top;
+ output->intake_suction_bottom = unsafe_goal->bottom;
+ }
+ had_piece_ = *has_piece;
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/superstructure/vacuum.h b/y2019/control_loops/superstructure/vacuum.h
new file mode 100644
index 0000000..3fd5a49
--- /dev/null
+++ b/y2019/control_loops/superstructure/vacuum.h
@@ -0,0 +1,55 @@
+#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
+#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
+
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "aos/controls/control_loop.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+class Vacuum {
+ public:
+ Vacuum() {}
+ void Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
+ SuperstructureQueue::Output *output, bool *has_piece,
+ aos::EventLoop *event_loop);
+
+
+ // Voltage to the vaccum pump when we are attempting to acquire a piece
+ static constexpr double kPumpVoltage = 8.0;
+
+ // Voltage to the vaccum pump when we have a piece
+ static constexpr double kPumpHasPieceVoltage = 2.0;
+
+ // Time to continue at the higher pump voltage after getting a gamepiece
+ static constexpr aos::monotonic_clock::duration kTimeAtHigherVoltage =
+ std::chrono::milliseconds(100);
+
+ // Time to continue the pump after getting a no suck goal
+ static constexpr aos::monotonic_clock::duration kTimeToKeepPumpRunning =
+ std::chrono::milliseconds(750);
+
+ private:
+ bool had_piece_ = false;
+ aos::monotonic_clock::time_point time_at_last_evacuate_goal_ =
+ aos::monotonic_clock::epoch();
+ aos::monotonic_clock::time_point time_at_last_acquisition_ =
+ aos::monotonic_clock::epoch();
+ double filtered_pressure_ = 1.0;
+
+ static constexpr double kVacuumThreshold = 0.70;
+
+ static constexpr double kFilterTimeConstant = 0.1;
+ static constexpr double dt = .00505;
+ static constexpr double kSuctionAlpha =
+ dt * (1 - kFilterTimeConstant) / (kFilterTimeConstant);
+
+ DISALLOW_COPY_AND_ASSIGN(Vacuum);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2019/joystick_angle.cc b/y2019/joystick_angle.cc
index 1923c53..90baf32 100644
--- a/y2019/joystick_angle.cc
+++ b/y2019/joystick_angle.cc
@@ -1,9 +1,20 @@
+#include <cmath>
+
+#include "frc971/zeroing/wrap.h"
#include "y2019/joystick_angle.h"
namespace y2019 {
namespace input {
namespace joysticks {
+using ::frc971::zeroing::Wrap;
+
+bool AngleCloseTo(double angle, double near, double range) {
+ double wrapped_angle = Wrap(near, angle, 2 * M_PI);
+
+ return ::std::abs(wrapped_angle - near) < range;
+}
+
JoystickAngle GetJoystickPosition(const JoystickAxis &x_axis,
const JoystickAxis &y_axis,
const Data &data) {
@@ -11,26 +22,30 @@
}
JoystickAngle GetJoystickPosition(float x_axis, float y_axis) {
- if (x_axis > kJoystickRight) {
- if (y_axis < kJoystickDown) {
- return JoystickAngle::kUpperRight;
- } else if (y_axis > kJoystickUp) {
- return JoystickAngle::kLowerRight;
- } else {
- return JoystickAngle::kMiddleRight;
- }
- } else if (x_axis < kJoystickLeft) {
- if (y_axis < kJoystickDown) {
- return JoystickAngle::kUpperLeft;
- } else if (y_axis > kJoystickUp) {
- return JoystickAngle::kLowerLeft;
- } else {
- return JoystickAngle::kMiddleLeft;
- }
+ const float magnitude = hypot(x_axis, y_axis);
+
+ if (magnitude < 0.5) {
+ return JoystickAngle::kDefault;
}
+
+ double angle = atan2(y_axis, x_axis);
+
+ if (AngleCloseTo(angle, M_PI / 3, M_PI / 6)) {
+ return JoystickAngle::kUpperRight;
+ } else if (AngleCloseTo(angle, 2 * M_PI / 3, M_PI / 6)) {
+ return JoystickAngle::kUpperLeft;
+ } else if (AngleCloseTo(angle, M_PI, M_PI / 6)) {
+ return JoystickAngle::kMiddleLeft;
+ } else if (AngleCloseTo(angle, 0, M_PI / 6)) {
+ return JoystickAngle::kMiddleRight;
+ } else if (AngleCloseTo(angle, -M_PI / 3, M_PI / 6)) {
+ return JoystickAngle::kLowerRight;
+ } else if (AngleCloseTo(angle, -2 * M_PI / 3, M_PI / 6)) {
+ return JoystickAngle::kLowerLeft;
+ }
+
return JoystickAngle::kDefault;
}
-
} // namespace joysticks
} // namespace input
} // namespace y2019
diff --git a/y2019/joystick_angle.h b/y2019/joystick_angle.h
index a2c7e36..09ab8af 100644
--- a/y2019/joystick_angle.h
+++ b/y2019/joystick_angle.h
@@ -9,12 +9,7 @@
namespace y2019 {
namespace input {
namespace joysticks {
-namespace {
-constexpr double kJoystickLeft = -0.5;
-constexpr double kJoystickRight = 0.5;
-constexpr double kJoystickUp = 0.5;
-constexpr double kJoystickDown = -0.5;
-}
+bool AngleCloseTo(double angle, double near, double range);
enum class JoystickAngle {
kDefault,
diff --git a/y2019/joystick_angle_test.cc b/y2019/joystick_angle_test.cc
index c352359..dc29004 100644
--- a/y2019/joystick_angle_test.cc
+++ b/y2019/joystick_angle_test.cc
@@ -7,12 +7,12 @@
using y2019::input::joysticks::GetJoystickPosition;
TEST(JoystickAngleTest, JoystickAngleTest) {
- EXPECT_EQ(JoystickAngle::kUpperRight, GetJoystickPosition(0.75, -0.75));
+ EXPECT_EQ(JoystickAngle::kUpperRight, GetJoystickPosition(0.75, 0.75));
EXPECT_EQ(JoystickAngle::kMiddleRight, GetJoystickPosition(0.75, 0));
- EXPECT_EQ(JoystickAngle::kLowerRight, GetJoystickPosition(0.75, 0.75));
- EXPECT_EQ(JoystickAngle::kUpperLeft, GetJoystickPosition(-0.75, -0.75));
+ EXPECT_EQ(JoystickAngle::kLowerRight, GetJoystickPosition(0.75, -0.75));
+ EXPECT_EQ(JoystickAngle::kUpperLeft, GetJoystickPosition(-0.75, 0.75));
EXPECT_EQ(JoystickAngle::kMiddleLeft, GetJoystickPosition(-0.75, 0));
- EXPECT_EQ(JoystickAngle::kLowerLeft, GetJoystickPosition(-0.75, 0.75));
+ EXPECT_EQ(JoystickAngle::kLowerLeft, GetJoystickPosition(-0.75, -0.75));
EXPECT_EQ(JoystickAngle::kDefault, GetJoystickPosition(0, 0));
}
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index b3969fb..2efb55b 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -23,11 +23,11 @@
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
+#include "aos/robot_state/robot_state.q.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/ADIS16448.h"
@@ -311,9 +311,17 @@
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- suction_victor_->SetSpeed(
- ::aos::Clip(queue->pump_voltage, -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
+ ::aos::robot_state.FetchLatest();
+ const double battery_voltage =
+ ::aos::robot_state.get() ? ::aos::robot_state->voltage_battery : 12.0;
+
+ // Throw a fast low pass filter on the battery voltage so we don't respond
+ // too fast to noise.
+ filtered_battery_voltage_ =
+ 0.5 * filtered_battery_voltage_ + 0.5 * battery_voltage;
+
+ suction_victor_->SetSpeed(::aos::Clip(
+ queue->pump_voltage / filtered_battery_voltage_, -1.0, 1.0));
}
void Stop() override {
@@ -328,6 +336,8 @@
::std::unique_ptr<::frc::VictorSP> elevator_victor_, intake_victor_,
wrist_victor_, stilts_victor_, suction_victor_;
+
+ double filtered_battery_voltage_ = 12.0;
};
class SolenoidWriter {
@@ -349,7 +359,7 @@
void set_intake_roller_talon(
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonSRX> t) {
intake_rollers_talon_ = ::std::move(t);
- intake_rollers_talon_->ConfigContinuousCurrentLimit(40.0, 0);
+ intake_rollers_talon_->ConfigContinuousCurrentLimit(20.0, 0);
intake_rollers_talon_->EnableCurrentLimit(true);
}