Add basic line-following drivetrain
Works by drawing a Nth degree polynomial to guide us to the target and
guiding a lateral controller to follow it given a speed provided by the
driver.
Change-Id: I94fd31666769ca398b66f04665f39e01cee00627
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index e332cfe..7dedc71 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -101,6 +101,43 @@
)
cc_library(
+ name = "line_follow_drivetrain",
+ srcs = [
+ "line_follow_drivetrain.cc",
+ ],
+ hdrs = [
+ "line_follow_drivetrain.h",
+ ],
+ deps = [
+ ":drivetrain_config",
+ ":drivetrain_queue",
+ "//aos:math",
+ "//aos/util:math",
+ "//frc971/control_loops:c2d",
+ "//frc971/control_loops:dlqr",
+ "//frc971/control_loops:pose",
+ "//third_party/eigen",
+ ],
+)
+
+cc_test(
+ name = "line_follow_drivetrain_test",
+ srcs = ["line_follow_drivetrain_test.cc"],
+ linkstatic = True,
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":drivetrain_config",
+ ":drivetrain_test_lib",
+ ":line_follow_drivetrain",
+ ":trajectory",
+ "//aos/testing:googletest",
+ "//aos/testing:test_shm",
+ "//third_party/matplotlib-cpp",
+ "@com_github_gflags_gflags//:gflags",
+ ],
+)
+
+cc_library(
name = "ssdrivetrain",
srcs = [
"ssdrivetrain.cc",
@@ -221,6 +258,7 @@
":down_estimator",
":drivetrain_queue",
":gear",
+ ":line_follow_drivetrain",
":localizer",
":polydrivetrain",
":splinedrivetrain",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 19e604d..2bfaaf5 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -41,6 +41,7 @@
dt_openloop_(dt_config_, &kf_),
dt_closedloop_(dt_config_, &kf_, localizer_),
dt_spline_(dt_config_),
+ dt_line_follow_(dt_config_),
down_estimator_(MakeDownEstimatorLoop()),
left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
@@ -247,17 +248,26 @@
dt_closedloop_.SetGoal(*goal);
dt_openloop_.SetGoal(*goal);
dt_spline_.SetGoal(*goal);
+ // TODO(james): Use a goal other than zero (which is what Pose default
+ // constructs to). Need to query the EKF in some way to determine what the
+ // logical target is.
+ const LineFollowDrivetrain::Pose goal_pose;
+ dt_line_follow_.SetGoal(*goal, goal_pose);
}
dt_openloop_.Update(robot_state().voltage_battery);
dt_closedloop_.Update(output != NULL && controller_type == 1);
- dt_spline_.Update(output != NULL && controller_type == 2,
- (Eigen::Matrix<double, 5, 1>() << localizer_->x(),
- localizer_->y(), localizer_->theta(),
- localizer_->left_velocity(), localizer_->right_velocity())
- .finished());
+ const Eigen::Matrix<double, 5, 1> trajectory_state =
+ (Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
+ localizer_->theta(), localizer_->left_velocity(),
+ localizer_->right_velocity())
+ .finished();
+
+ dt_spline_.Update(output != NULL && controller_type == 2, trajectory_state);
+
+ dt_line_follow_.Update(trajectory_state);
switch (controller_type) {
case 0:
@@ -269,6 +279,9 @@
case 2:
dt_spline_.SetOutput(output);
break;
+ case 3:
+ dt_line_follow_.SetOutput(output);
+ break;
}
// The output should now contain the shift request.
@@ -310,6 +323,7 @@
dt_openloop_.PopulateStatus(status);
dt_closedloop_.PopulateStatus(status);
dt_spline_.PopulateStatus(status);
+ dt_line_follow_.PopulateStatus(status);
}
double left_voltage = 0.0;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 1aa3e13..35d6084 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -12,6 +12,7 @@
#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/line_follow_drivetrain.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
namespace frc971 {
@@ -50,6 +51,7 @@
PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
SplineDrivetrain dt_spline_;
+ LineFollowDrivetrain dt_line_follow_;
::aos::monotonic_clock::time_point last_gyro_time_ =
::aos::monotonic_clock::min_time;
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 04bdaaf..66e9c32 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -81,7 +81,8 @@
// Type of controller in charge of the drivetrain.
// 0: polydrive
// 1: motion profiled position drive (statespace)
- // 2: spline follower.
+ // 2: spline follower
+ // 3: line follower (for guiding into a target)
uint8_t controller_type;
// Position goals for each drivetrain side (in meters) when the
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 7ce34e5..2917bc6 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -719,6 +719,21 @@
VerifyNearSplineGoal();
}
+// The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
+// tests that the integration with drivetrain_lib worked properly.
+TEST_F(DrivetrainTest, BasicLineFollow) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 3;
+ goal->throttle = 0.5;
+ goal.Send();
+
+ RunForTime(chrono::seconds(5));
+ // Should be on the x-axis, with y = 0 and x having increased:
+ EXPECT_LT(0.5, drivetrain_motor_plant_.GetPosition().x());
+ EXPECT_NEAR(0.0, drivetrain_motor_plant_.GetPosition().y(), 1e-4);
+}
+
::aos::controls::HVPolytope<2, 4, 4> MakeBox(double x1_min, double x1_max,
double x2_min, double x2_max) {
Eigen::Matrix<double, 4, 2> box_H;
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
new file mode 100644
index 0000000..e9a6c70
--- /dev/null
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -0,0 +1,212 @@
+#include <iostream>
+#include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
+
+#include "aos/commonmath.h"
+#include "aos/util/math.h"
+#include "frc971/control_loops/c2d.h"
+#include "frc971/control_loops/dlqr.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+constexpr double LineFollowDrivetrain::kMaxVoltage;
+constexpr double LineFollowDrivetrain::kPolyN;
+
+namespace {
+::Eigen::Matrix<double, 3, 3> AContinuous(
+ const DrivetrainConfig<double> &dt_config) {
+ ::Eigen::Matrix<double, 3, 3> result;
+ result.setZero();
+ result(0, 2) = 1.0;
+ result.block<2, 2>(1, 1) = dt_config.Tlr_to_la() *
+ dt_config.make_hybrid_drivetrain_velocity_loop()
+ .plant()
+ .coefficients()
+ .A_continuous *
+ dt_config.Tla_to_lr();
+ return result;
+}
+::Eigen::Matrix<double, 3, 2> BContinuous(
+ const DrivetrainConfig<double> &dt_config) {
+ ::Eigen::Matrix<double, 3, 2> result;
+ result.setZero();
+ result.block<2, 2>(1, 0) = dt_config.Tlr_to_la() *
+ dt_config.make_hybrid_drivetrain_velocity_loop()
+ .plant()
+ .coefficients()
+ .B_continuous;
+ return result;
+}
+void AB(const DrivetrainConfig<double> &dt_config,
+ ::Eigen::Matrix<double, 3, 3> *A, ::Eigen::Matrix<double, 3, 2> *B) {
+ controls::C2D(AContinuous(dt_config), BContinuous(dt_config), dt_config.dt, A,
+ B);
+}
+
+::Eigen::Matrix<double, 3, 3> ADiscrete(
+ const DrivetrainConfig<double> &dt_config) {
+ ::Eigen::Matrix<double, 3, 3> ADiscrete;
+ ::Eigen::Matrix<double, 3, 2> BDiscrete;
+ AB(dt_config, &ADiscrete, &BDiscrete);
+ return ADiscrete;
+}
+
+::Eigen::Matrix<double, 3, 2> BDiscrete(
+ const DrivetrainConfig<double> &dt_config) {
+ ::Eigen::Matrix<double, 3, 3> ADiscrete;
+ ::Eigen::Matrix<double, 3, 2> BDiscrete;
+ AB(dt_config, &ADiscrete, &BDiscrete);
+ return BDiscrete;
+}
+
+::Eigen::Matrix<double, 2, 3> CalcK(
+ const ::Eigen::Matrix<double, 3, 3> & A,
+ const ::Eigen::Matrix<double, 3, 2> & B,
+ const ::Eigen::Matrix<double, 3, 3> & Q,
+ const ::Eigen::Matrix<double, 2, 2> & R) {
+ Eigen::Matrix<double, 2, 3> K;
+ Eigen::Matrix<double, 3, 3> S;
+ int info = ::frc971::controls::dlqr<3, 2>(A, B, Q, R, &K, &S);
+ if (info != 0) {
+ // We allow a FATAL error here since this should only be called during
+ // initialization.
+ LOG(FATAL, "Failed to solve %d, controllability: %d\n", info,
+ controls::Controllability(A, B));
+ }
+ return K;
+}
+
+::Eigen::Matrix<double, 2, 3> CalcKff(const ::Eigen::Matrix<double, 3, 2> &B) {
+ ::Eigen::Matrix<double, 2, 3> Kff;
+ Kff.setZero();
+ Kff.block<2, 2>(0, 1) = B.block<2, 2>(1, 0).inverse();
+ return Kff;
+}
+
+} // namespace
+
+// When we create A/B, we do recompute A/B, but we don't really care about
+// optimizing it since it is only happening at initialization time...
+LineFollowDrivetrain::LineFollowDrivetrain(
+ const DrivetrainConfig<double> &dt_config)
+ : dt_config_(dt_config),
+ A_d_(ADiscrete(dt_config_)),
+ B_d_(BDiscrete(dt_config_)),
+ K_(CalcK(A_d_, B_d_, Q_, R_)),
+ Kff_(CalcKff(B_d_)) {}
+
+double LineFollowDrivetrain::GoalTheta(
+ const ::Eigen::Matrix<double, 5, 1> &state) const {
+ // TODO(james): Consider latching the sign of the goal so that the driver
+ // can back up without the robot trying to turn around...
+ // On the other hand, we may just want to force the driver to take over
+ // entirely if they need to backup.
+ int sign = ::aos::sign(goal_velocity_);
+ // Given a Nth degree polynomial with just a single term that
+ // has its minimum (with a slope of zero) at (0, 0) and passing
+ // through (x0, y0), we will have:
+ // y = a * x^N
+ // a = y0 / x0^N
+ // And the slope of the tangent line at (x0, y0) will be
+ // N * a * x0^(N-1)
+ // = N * y0 / x0^N * x0^(N-1)
+ // = N * y0 / x0
+ // Giving a heading of
+ // atan2(-N * y0, -x0)
+ // where we add the negative signs to make things work out properly when we
+ // are trying to drive forwards towards zero. We reverse the sign of both
+ // terms if we want to drive backwards.
+ return ::std::atan2(-sign * kPolyN * state.y(), -sign * state.x());
+}
+
+double LineFollowDrivetrain::GoalThetaDot(
+ const ::Eigen::Matrix<double, 5, 1> &state) const {
+ // theta = atan2(-N * y0, -x0)
+ // Note: d(atan2(p(t), q(t)))/dt
+ // = (p(t) * q'(t) - q(t) * p'(t)) / (p(t)^2 + q(t)^2)
+ // Note: x0' = cos(theta) * v, y0' = sin(theta) * v
+ // Note that for the sin/cos calculations we discard the
+ // negatives in the arctangent to make the signs work out (the
+ // dtheta/dt needs to be correct as we travel along the path). This
+ // also corresponds with the fact that thetadot is agnostic towards
+ // whether the robot is driving forwards or backwards, so long as it is
+ // trying to drive towards the target.
+ // Note: sin(theta) = sin(atan2(N * y0, x0))
+ // = N * y0 / sqrt(N^2 * y0^2 + x0^2)
+ // Note: cos(theta) = cos(atan2(N * y0, x0))
+ // = x0 / sqrt(N^2 * y0^2 + x0^2)
+ // dtheta/dt
+ // = (-x0 * (-N * y0') - -N * y0 * (-x0')) / (N^2 * y0^2 + x0^2)
+ // = N * (x0 * v * sin(theta) - y0 * v * cos(theta)) / (N^2 * y0^2 + x0^2)
+ // = N * v * (x0 * (N * y0) - y0 * (x0)) / (N^2 * y0^2 + x0^2)^1.5
+ // = N * v * (N - 1) * x0 * y0 / (N^2 * y0^2 + x0^2)^1.5
+ const double linear_vel = (state(3, 0) + state(4, 0)) / 2.0;
+ const double x2 = ::std::pow(state.x(), 2);
+ const double y2 = ::std::pow(state.y(), 2);
+ const double norm = y2 * kPolyN * kPolyN + x2;
+ // When we get too near the goal, avoid singularity in a sane manner.
+ if (norm < 1e-3) {
+ return 0.0;
+ }
+ return kPolyN * (kPolyN - 1) * linear_vel * state.x() * state.y() /
+ (norm * ::std::sqrt(norm));
+}
+
+void LineFollowDrivetrain::SetGoal(
+ const ::frc971::control_loops::DrivetrainQueue::Goal &goal,
+ const Pose &goal_pose) {
+ // TODO(james): More properly calculate goal velocity from throttle.
+ goal_velocity_ = goal.throttle;
+ target_pose_ = goal_pose;
+}
+
+void LineFollowDrivetrain::SetOutput(
+ ::frc971::control_loops::DrivetrainQueue::Output *output) {
+ // TODO(james): Account for voltage error terms, and/or provide driver with
+ // ability to influence steering.
+ if (output != nullptr) {
+ output->left_voltage = U_(0, 0);
+ output->right_voltage = U_(1, 0);
+ }
+}
+
+void LineFollowDrivetrain::Update(
+ const ::Eigen::Matrix<double, 5, 1> &abs_state) {
+ // Get the robot pose in the target coordinate frame.
+ const Pose relative_robot_pose = Pose({abs_state.x(), abs_state.y(), 0.0},
+ abs_state(2, 0)).Rebase(&target_pose_);
+ // Always force a slight negative X, so that the driver can continue to drive
+ // past zero if they want.
+ // The "slight negative" needs to be large enough that we won't force
+ // obnoxiously sharp turning radii if we run past the end of the
+ // line--because, in practice, the driver should never be trying to drive to a
+ // target where they are significantly off laterally at <0.1m from the target,
+ // this should not be a problem.
+ const double relative_x = ::std::min(relative_robot_pose.rel_pos().x(), -0.1);
+ const ::Eigen::Matrix<double, 5, 1> rel_state =
+ (::Eigen::Matrix<double, 5, 1>() << relative_x,
+ relative_robot_pose.rel_pos().y(), relative_robot_pose.rel_theta(),
+ abs_state(3, 0), abs_state(4, 0))
+ .finished();
+ ::Eigen::Matrix<double, 3, 1> controls_goal(
+ GoalTheta(rel_state), goal_velocity_, GoalThetaDot(rel_state));
+ ::Eigen::Matrix<double, 3, 1> controls_state;
+ controls_state(0, 0) = rel_state(2, 0);
+ controls_state.block<2, 1>(1, 0) =
+ dt_config_.Tlr_to_la() * rel_state.block<2, 1>(3, 0);
+ ::Eigen::Matrix<double, 3, 1> controls_err = controls_goal - controls_state;
+ // Because we are taking the difference of an angle, normaliez to [-pi, pi].
+ controls_err(0, 0) = ::aos::math::NormalizeAngle(controls_err(0, 0));
+ // TODO(james): Calculate the next goal so that we are properly doing
+ // feed-forwards.
+ ::Eigen::Matrix<double, 2, 1> U_ff =
+ Kff_ * (controls_goal - A_d_ * controls_goal);
+ U_ = K_ * controls_err + U_ff;
+ const double maxU = U_.lpNorm<::Eigen::Infinity>();
+ U_ *= (maxU > kMaxVoltage) ? kMaxVoltage / maxU : 1.0;
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
new file mode 100644
index 0000000..eb6c3a2
--- /dev/null
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -0,0 +1,93 @@
+#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
+#define FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
+#include "Eigen/Dense"
+
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+
+namespace testing {
+class LineFollowDrivetrainTest;
+} // namespace testing
+
+// A drivetrain that permits a velocity input from the driver while controlling
+// lateral motion.
+// TODO(james): Do we want to allow some driver input on lateral control? It may
+// just be too confusing to make work effectively, but it also wouldn't be too
+// hard to just allow the driver to specify an offset to the angle/angular
+// velocity.
+class LineFollowDrivetrain {
+ public:
+ typedef TypedPose<double> Pose;
+
+ LineFollowDrivetrain(const DrivetrainConfig<double> &dt_config);
+ // Sets the current goal; the drivetrain queue contains throttle_velocity
+ // which is used to command overall robot velocity. The goal_pose is a Pose
+ // representing where we are tring to go. This would typically be the Pose of
+ // a Target; the positive X-axis in the Pose's frame represents the direction
+ // we want to go (we approach the pose from the left-half plane).
+ void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal,
+ const Pose &goal_pose);
+ // State: [x, y, theta, left_vel, right_vel]
+ void Update(const ::Eigen::Matrix<double, 5, 1> &state);
+ void SetOutput(
+ ::frc971::control_loops::DrivetrainQueue::Output *output);
+ // TODO(james): Populate.
+ void PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status * /*status*/) const {}
+
+ private:
+ // Nominal max voltage.
+ // TODO(james): Is there a config for this or anything?
+ static constexpr double kMaxVoltage = 12.0;
+ // Degree of the polynomial to follow in. Should be strictly greater than 1.
+ // A value of 1 would imply that we drive straight to the target (but not hit
+ // it straight on, unless we happened to start right in front of it). A value
+ // of zero would imply driving straight until we hit the plane of the target.
+ // Values between 0 and 1 would imply hitting the target from the side.
+ static constexpr double kPolyN = 3.0;
+ static_assert(kPolyN > 1.0,
+ "We want to hit targets straight on (see above comments).");
+
+ double GoalTheta(const ::Eigen::Matrix<double, 5, 1> &state) const;
+ double GoalThetaDot(const ::Eigen::Matrix<double, 5, 1> &state) const;
+
+ const DrivetrainConfig<double> dt_config_;
+ // TODO(james): These should probably be factored out somewhere.
+ const ::Eigen::DiagonalMatrix<double, 3> Q_ =
+ (::Eigen::DiagonalMatrix<double, 3>().diagonal()
+ << 1.0 / ::std::pow(0.01, 2),
+ 1.0 / ::std::pow(0.1, 2), 1.0 / ::std::pow(10.0, 2))
+ .finished()
+ .asDiagonal();
+ const ::Eigen::DiagonalMatrix<double, 2> R_ =
+ (::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 1.0 / ::std::pow(12.0, 2),
+ 1.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal();
+ // The matrices we use for the linear controller.
+ // State for these is [theta, linear_velocity, angular_velocity]
+ const ::Eigen::Matrix<double, 3, 3> A_d_;
+ const ::Eigen::Matrix<double, 3, 2> B_d_;
+ const ::Eigen::Matrix<double, 2, 3> K_;
+ const ::Eigen::Matrix<double, 2, 3> Kff_;
+
+ Pose target_pose_;
+ double goal_velocity_ = 0.0;
+
+ // Voltage output to apply
+ ::Eigen::Matrix<double, 2, 1> U_;
+
+ friend class testing::LineFollowDrivetrainTest;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
new file mode 100644
index 0000000..44d1e31
--- /dev/null
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -0,0 +1,299 @@
+#include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
+
+#include <chrono>
+
+#include "aos/testing/test_shm.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "frc971/control_loops/drivetrain/trajectory.h"
+#include "gflags/gflags.h"
+#include "gtest/gtest.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+
+DEFINE_bool(plot, false, "If true, plot");
+
+namespace chrono = ::std::chrono;
+
+namespace frc971 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+class LineFollowDrivetrainTest : public ::testing::Test {
+ public:
+ typedef TypedPose<double> Pose;
+
+ ::aos::testing::TestSharedMemory shm_;
+
+ LineFollowDrivetrainTest()
+ : config_(GetTestDrivetrainConfig()),
+ drivetrain_(config_),
+ 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>>(
+ config_.make_hybrid_drivetrain_velocity_loop()))),
+ Tlr_to_la_(config_.Tlr_to_la()),
+ Tla_to_lr_(config_.Tla_to_lr()) {}
+
+ void set_goal_pose(const Pose &pose) {
+ goal_pose_ = pose;
+ }
+
+ void RunForTime(chrono::nanoseconds t) {
+ const int niter = t / config_.dt;
+ for (int ii = 0; ii < niter; ++ii) {
+ Iterate();
+ }
+ }
+
+ void Iterate() {
+ ::frc971::control_loops::DrivetrainQueue::Goal goal;
+ goal.throttle = driver_model_(state_);
+ drivetrain_.SetGoal(goal, goal_pose_);
+ drivetrain_.Update(state_);
+
+ ::frc971::control_loops::DrivetrainQueue::Output output;
+ drivetrain_.SetOutput(&output);
+
+ EXPECT_LE(::std::abs(output.left_voltage), 12.0 + 1e-6);
+ EXPECT_LE(::std::abs(output.right_voltage), 12.0 + 1e-6);
+
+ ::Eigen::Matrix<double, 2, 1> U(output.left_voltage, output.right_voltage);
+
+ state_ = RungeKuttaU(
+ [this](const ::Eigen::Matrix<double, 5, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ return ContinuousDynamics(velocity_drivetrain_->plant(), Tlr_to_la_,
+ X, U);
+ },
+ state_, U,
+ chrono::duration_cast<chrono::duration<double>>(config_.dt).count());
+ t_ += config_.dt;
+
+ time_.push_back(chrono::duration_cast<chrono::duration<double>>(
+ t_.time_since_epoch()).count());
+ simulation_ul_.push_back(U(0, 0));
+ simulation_ur_.push_back(U(1, 0));
+ simulation_x_.push_back(state_.x());
+ simulation_y_.push_back(state_.y());
+ }
+
+ // Check that left/right velocities are near zero and the absolute position is
+ // near that of the goal Pose.
+ void VerifyNearGoal() const {
+ EXPECT_NEAR(goal_pose_.abs_pos().x(), state_(0, 0), 1e-3);
+ EXPECT_NEAR(goal_pose_.abs_pos().y(), state_(1, 0), 1e-3);
+ // state should be at 0 or PI (we should've come in striaght on or straight
+ // backwards).
+ const double angle_err =
+ ::aos::math::DiffAngle(goal_pose_.abs_theta(), state_(2, 0));
+ const double angle_pi_err = ::aos::math::DiffAngle(angle_err, M_PI);
+ EXPECT_LT(::std::min(::std::abs(angle_err), ::std::abs(angle_pi_err)),
+ 1e-3);
+ // Left/right velocities are zero:
+ EXPECT_NEAR(0.0, state_(3, 0), 1e-3);
+ EXPECT_NEAR(0.0, state_(4, 0), 1e-3);
+ }
+
+ void CheckGoalTheta(double x, double y, double v, double expected_theta,
+ double expected_thetadot) {
+ ::Eigen::Matrix<double, 5, 1> state;
+ state << x, y, 0.0, v, v;
+ const double theta = drivetrain_.GoalTheta(state);
+ const double thetadot = drivetrain_.GoalThetaDot(state);
+ EXPECT_EQ(expected_theta, theta) << "x " << x << " y " << y << " v " << v;
+ EXPECT_EQ(expected_thetadot, thetadot)
+ << "x " << x << " y " << y << " v " << v;
+ }
+
+ void CheckGoalThetaDotAtState(double x, double y, double v) {
+ ::Eigen::Matrix<double, 5, 1> state;
+ state << x, y, 0.0, v, v;
+ const double theta = drivetrain_.GoalTheta(state);
+ const double thetadot = drivetrain_.GoalThetaDot(state);
+ const double dx = v * ::std::cos(theta);
+ const double dy = v * ::std::sin(theta);
+ constexpr double kEps = 1e-5;
+ state(0, 0) += dx * kEps;
+ state(1, 0) += dy * kEps;
+ const double next_theta = drivetrain_.GoalTheta(state);
+ EXPECT_NEAR(thetadot, ::aos::math::DiffAngle(next_theta, theta) / kEps,
+ 1e-4)
+ << "theta: " << theta << " nexttheta: " << next_theta << " x " << x
+ << " y " << y << " v " << v;
+ }
+
+ void TearDown() override {
+ if (FLAGS_plot) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(time_, simulation_ul_, {{"label", "ul"}});
+ matplotlibcpp::plot(time_, simulation_ur_, {{"label", "ur"}});
+ matplotlibcpp::legend();
+
+ Pose base_pose(&goal_pose_, {-1.0, 0.0, 0.0}, 0.0);
+ ::std::vector<double> line_x(
+ {base_pose.abs_pos().x(), goal_pose_.abs_pos().x()});
+ ::std::vector<double> line_y(
+ {base_pose.abs_pos().y(), goal_pose_.abs_pos().y()});
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(line_x, line_y, {{"label", "line"}, {"marker", "o"}});
+ matplotlibcpp::plot(simulation_x_, simulation_y_,
+ {{"label", "robot"}, {"marker", "o"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::show();
+ }
+ }
+
+ protected:
+ // Strategy used by the driver to command throttle.
+ // This function should return a throttle to apply given the current state.
+ // Default to simple proportional gain:
+ ::std::function<double(::Eigen::Matrix<double, 5, 1>)> driver_model_ =
+ [](const ::Eigen::Matrix<double, 5, 1> &state) {
+ return -5.0 * state.x();
+ };
+
+ // Current state; [x, y, theta, left_velocity, right_velocity]
+ ::Eigen::Matrix<double, 5, 1> state_;
+
+ private:
+ const DrivetrainConfig<double> config_;
+
+ LineFollowDrivetrain drivetrain_;
+
+ ::std::unique_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>
+ velocity_drivetrain_;
+
+ // Transformation matrix from left, right to linear, angular
+ const ::Eigen::Matrix<double, 2, 2> Tlr_to_la_;
+ // Transformation matrix from linear, angular to left, right
+ const ::Eigen::Matrix<double, 2, 2> Tla_to_lr_;
+
+ // Current goal pose we are trying to drive to.
+ Pose goal_pose_;
+
+ aos::monotonic_clock::time_point t_;
+
+ // Debugging vectors for plotting:
+ ::std::vector<double> time_;
+ ::std::vector<double> simulation_ul_;
+ ::std::vector<double> simulation_ur_;
+ ::std::vector<double> simulation_x_;
+ ::std::vector<double> simulation_y_;
+};
+
+TEST_F(LineFollowDrivetrainTest, ValidGoalThetaDot) {
+ for (double x : {1.0, 0.0, -1.0, -2.0, -3.0}) {
+ for (double y : {-3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0}) {
+ for (double v : {-3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0}) {
+ if (x == 0.0 && y == 0.0) {
+ // When x/y are zero, we can encounter singularities. The code should
+ // just provide zeros in this case.
+ CheckGoalTheta(x, y, v, 0.0, 0.0);
+ } else {
+ CheckGoalThetaDotAtState(x, y, v);
+ }
+ }
+ }
+ }
+}
+
+// If the driver commands the robot to keep going, we should just run straight
+// off the end and keep going along the line:
+TEST_F(LineFollowDrivetrainTest, RunOffEnd) {
+ state_ << -1.0, 0.1, 0.0, 0.0, 0.0;
+ driver_model_ = [](const ::Eigen::Matrix<double, 5, 1> &) { return 1.0; };
+ RunForTime(chrono::seconds(10));
+ EXPECT_LT(6.0, state_.x());
+ EXPECT_NEAR(0.0, state_.y(), 1e-4);
+ EXPECT_NEAR(0.0, state_(2, 0), 1e-4);
+ EXPECT_NEAR(1.0, state_(3, 0), 1e-4);
+ EXPECT_NEAR(1.0, state_(4, 0), 1e-4);
+}
+
+class LineFollowDrivetrainTargetParamTest
+ : public LineFollowDrivetrainTest,
+ public ::testing::WithParamInterface<Pose> {};
+TEST_P(LineFollowDrivetrainTargetParamTest, NonZeroTargetTest) {
+ // Start the state at zero and then put the target in a
+ state_.setZero();
+ driver_model_ = [this](const ::Eigen::Matrix<double, 5, 1> &state) {
+ return (state.topRows<2>() - GetParam().abs_pos().topRows<2>()).norm();
+ };
+ set_goal_pose(GetParam());
+ RunForTime(chrono::seconds(10));
+ VerifyNearGoal();
+}
+INSTANTIATE_TEST_CASE_P(TargetPosTest, LineFollowDrivetrainTargetParamTest,
+ ::testing::Values(Pose({0.0, 0.0, 0.0}, 0.0),
+ Pose({1.0, 0.0, 0.0}, 0.0),
+ Pose({3.0, 1.0, 0.0}, 0.0),
+ Pose({3.0, 0.0, 0.0}, 0.5),
+ Pose({3.0, 0.0, 0.0}, -0.5),
+ Pose({-3.0, -1.0, 0.0}, -2.5)));
+
+class LineFollowDrivetrainParamTest
+ : public LineFollowDrivetrainTest,
+ public ::testing::WithParamInterface<::std::tuple<
+ ::Eigen::Matrix<double, 5, 1>,
+ ::std::function<double(const ::Eigen::Matrix<double, 5, 1> &)>>> {};
+
+TEST_P(LineFollowDrivetrainParamTest, VaryPositionAndModel) {
+ state_ = ::std::get<0>(GetParam());
+ driver_model_ = ::std::get<1>(GetParam());
+ RunForTime(chrono::seconds(10));
+ VerifyNearGoal();
+}
+
+INSTANTIATE_TEST_CASE_P(
+ PositionAndModelTest, LineFollowDrivetrainParamTest,
+ ::testing::Combine(
+ ::testing::Values(
+ (::Eigen::Matrix<double, 5, 1>() << -1.0, 0.0, 0.0, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, 0.0, 0.0, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, 0.4, 0.0, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, -0.4, 0.0, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, 0.0, 0.5, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, 0.0, -0.5, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, 1.0, 0.5, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, 1.0, -0.5, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, -1.0, 0.5, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, -1.0, -0.5, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, 1.0, -2.0, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, -1.0, 2.0, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, -1.0, 12.0, 0.0, 0.0)
+ .finished(),
+ (::Eigen::Matrix<double, 5, 1>() << -2.0, -1.0, 0.0, 0.5, -0.5)
+ .finished()),
+ ::testing::Values(
+ [](const ::Eigen::Matrix<double, 5, 1> &state) {
+ return -5.0 * state.x();
+ },
+ [](const ::Eigen::Matrix<double, 5, 1> &state) {
+ return 5.0 * state.x();
+ },
+ [](const ::Eigen::Matrix<double, 5, 1> &state) {
+ return -1.0 * ::std::abs(state.x()) - 0.5 * state.x() * state.x();
+ })));
+
+} // namespace testing
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace frc971