Add interface for target selector
This adds an interface to provide something to choose a target to guide
into based on the current robot state.
This does not actually add a real implementation for target choosing.
Change-Id: I73bfde640dbe0851847f9f77710614607bad96ea
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 7dedc71..c99053f 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -71,6 +71,7 @@
deps = [
":drivetrain_config",
":hybrid_ekf",
+ "//frc971/control_loops:pose",
],
)
@@ -111,6 +112,7 @@
deps = [
":drivetrain_config",
":drivetrain_queue",
+ ":localizer",
"//aos:math",
"//aos/util:math",
"//frc971/control_loops:c2d",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2bfaaf5..b6b8b5d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -41,7 +41,7 @@
dt_openloop_(dt_config_, &kf_),
dt_closedloop_(dt_config_, &kf_, localizer_),
dt_spline_(dt_config_),
- dt_line_follow_(dt_config_),
+ dt_line_follow_(dt_config_, localizer->target_selector()),
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),
@@ -248,11 +248,7 @@
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_line_follow_.SetGoal(*goal);
}
dt_openloop_.Update(robot_state().voltage_battery);
@@ -280,7 +276,11 @@
dt_spline_.SetOutput(output);
break;
case 3:
- dt_line_follow_.SetOutput(output);
+ if (!dt_line_follow_.SetOutput(output)) {
+ // If the line follow drivetrain was unable to execute (generally due to
+ // not having a target), execute the regular teleop drivetrain.
+ dt_openloop_.SetOutput(output);
+ }
break;
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 2917bc6..97aaba5 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -722,6 +722,8 @@
// The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
// tests that the integration with drivetrain_lib worked properly.
TEST_F(DrivetrainTest, BasicLineFollow) {
+ localizer_.target_selector()->set_has_target(true);
+ localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
my_drivetrain_queue_.goal.MakeMessage();
goal->controller_type = 3;
@@ -729,8 +731,27 @@
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());
+ // Should have run off the end of the target, running along the y=x line.
+ EXPECT_LT(1.0, drivetrain_motor_plant_.GetPosition().x());
+ EXPECT_NEAR(drivetrain_motor_plant_.GetPosition().x(),
+ drivetrain_motor_plant_.GetPosition().y(), 1e-4);
+}
+
+// Tests that the line follower will not run and defer to regular open-loop
+// driving when there is no target yet:
+TEST_F(DrivetrainTest, LineFollowDefersToOpenLoop) {
+ localizer_.target_selector()->set_has_target(false);
+ localizer_.target_selector()->set_pose({{1.0, 1.0, 0.0}, M_PI_4});
+ ::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 have run straight (because we just set throttle, with wheel = 0)
+ // along X-axis.
+ EXPECT_LT(1.0, drivetrain_motor_plant_.GetPosition().x());
EXPECT_NEAR(0.0, drivetrain_motor_plant_.GetPosition().y(), 1e-4);
}
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index e9a6c70..c4ca5ef 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -89,12 +89,15 @@
// 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)
+ const DrivetrainConfig<double> &dt_config,
+ TargetSelectorInterface *target_selector)
: 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_)) {}
+ Kff_(CalcKff(B_d_)),
+ target_selector_(target_selector),
+ U_(0, 0) {}
double LineFollowDrivetrain::GoalTheta(
const ::Eigen::Matrix<double, 5, 1> &state) const {
@@ -154,25 +157,48 @@
}
void LineFollowDrivetrain::SetGoal(
- const ::frc971::control_loops::DrivetrainQueue::Goal &goal,
- const Pose &goal_pose) {
+ const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
// TODO(james): More properly calculate goal velocity from throttle.
goal_velocity_ = goal.throttle;
- target_pose_ = goal_pose;
+ // Freeze the target once the driver presses the button; if we haven't yet
+ // confirmed a target when the driver presses the button, we will not do
+ // anything and report not ready until we have a target.
+ freeze_target_ = goal.controller_type == 3;
}
-void LineFollowDrivetrain::SetOutput(
+bool 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) {
+ if (output != nullptr && have_target_) {
output->left_voltage = U_(0, 0);
output->right_voltage = U_(1, 0);
}
+ return have_target_;
}
void LineFollowDrivetrain::Update(
const ::Eigen::Matrix<double, 5, 1> &abs_state) {
+ // Because we assume the target selector may have some internal state (e.g.,
+ // not confirming a target until some time as passed), we should call
+ // UpdateSelection every time.
+ bool new_target = target_selector_->UpdateSelection(abs_state);
+ if (freeze_target_) {
+ // When freezing the target, only make changes if we didn't have a good
+ // target before.
+ if (!have_target_ && new_target) {
+ have_target_ = true;
+ target_pose_ = target_selector_->TargetPose();
+ }
+ } else {
+ // If the target selector has lost its target, we *do* want to set
+ // have_target_ to false, so long as we aren't trying to freeze the target.
+ have_target_ = new_target;
+ if (have_target_) {
+ target_pose_ = target_selector_->TargetPose();
+ }
+ }
+
// 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_);
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index eb6c3a2..83fba20 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -5,6 +5,7 @@
#include "frc971/control_loops/pose.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 {
@@ -24,17 +25,21 @@
public:
typedef TypedPose<double> Pose;
- LineFollowDrivetrain(const DrivetrainConfig<double> &dt_config);
+ LineFollowDrivetrain(
+ const DrivetrainConfig<double> &dt_config,
+ TargetSelectorInterface *target_selector);
// 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);
+ void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
// State: [x, y, theta, left_vel, right_vel]
void Update(const ::Eigen::Matrix<double, 5, 1> &state);
- void SetOutput(
+ // Returns whether we set the output. If false, that implies that we do not
+ // yet have a target to track into and so some other drivetrain should take
+ // over.
+ bool SetOutput(
::frc971::control_loops::DrivetrainQueue::Output *output);
// TODO(james): Populate.
void PopulateStatus(
@@ -77,6 +82,9 @@
const ::Eigen::Matrix<double, 2, 3> K_;
const ::Eigen::Matrix<double, 2, 3> Kff_;
+ TargetSelectorInterface *target_selector_;
+ bool freeze_target_ = false;
+ bool have_target_ = false;
Pose target_pose_;
double goal_velocity_ = 0.0;
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index 44d1e31..65cf9bf 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -26,20 +26,21 @@
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()))),
+ drivetrain_(config_, &target_selector_),
+ 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 set_goal_pose(const Pose &pose) { target_selector_.set_pose(pose); }
+
+ Pose goal_pose() const { return target_selector_.TargetPose(); }
void RunForTime(chrono::nanoseconds t) {
const int niter = t / config_.dt;
@@ -51,11 +52,17 @@
void Iterate() {
::frc971::control_loops::DrivetrainQueue::Goal goal;
goal.throttle = driver_model_(state_);
- drivetrain_.SetGoal(goal, goal_pose_);
+ goal.controller_type = freeze_target_ ? 3 : 0;
+ drivetrain_.SetGoal(goal);
drivetrain_.Update(state_);
::frc971::control_loops::DrivetrainQueue::Output output;
- drivetrain_.SetOutput(&output);
+ EXPECT_EQ(expect_output_, drivetrain_.SetOutput(&output));
+ if (!expect_output_) {
+ EXPECT_EQ(0.0, output.left_voltage);
+ EXPECT_EQ(0.0, output.right_voltage);
+ }
+
EXPECT_LE(::std::abs(output.left_voltage), 12.0 + 1e-6);
EXPECT_LE(::std::abs(output.right_voltage), 12.0 + 1e-6);
@@ -83,12 +90,12 @@
// 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);
+ 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));
+ ::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);
@@ -132,11 +139,12 @@
matplotlibcpp::plot(time_, simulation_ur_, {{"label", "ur"}});
matplotlibcpp::legend();
- Pose base_pose(&goal_pose_, {-1.0, 0.0, 0.0}, 0.0);
+ TypedPose<double> target = goal_pose();
+ Pose base_pose(&target, {-1.0, 0.0, 0.0}, 0.0);
::std::vector<double> line_x(
- {base_pose.abs_pos().x(), goal_pose_.abs_pos().x()});
+ {base_pose.abs_pos().x(), target.abs_pos().x()});
::std::vector<double> line_y(
- {base_pose.abs_pos().y(), goal_pose_.abs_pos().y()});
+ {base_pose.abs_pos().y(), target.abs_pos().y()});
matplotlibcpp::figure();
matplotlibcpp::plot(line_x, line_y, {{"label", "line"}, {"marker", "o"}});
matplotlibcpp::plot(simulation_x_, simulation_y_,
@@ -157,7 +165,11 @@
};
// Current state; [x, y, theta, left_velocity, right_velocity]
- ::Eigen::Matrix<double, 5, 1> state_;
+ ::Eigen::Matrix<double, 5, 1> state_ = ::Eigen::Matrix<double, 5, 1>::Zero();
+ TrivialTargetSelector target_selector_;
+
+ bool freeze_target_ = false;
+ bool expect_output_ = true;
private:
const DrivetrainConfig<double> config_;
@@ -174,9 +186,6 @@
// 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:
@@ -216,6 +225,66 @@
EXPECT_NEAR(1.0, state_(4, 0), 1e-4);
}
+// Tests that the outputs are not set when there is no valid target.
+TEST_F(LineFollowDrivetrainTest, DoesntHaveTarget) {
+ state_.setZero();
+ target_selector_.set_has_target(false);
+ expect_output_ = false;
+ // There are checks in Iterate for the logic surrounding SetOutput().
+ RunForTime(::std::chrono::seconds(5));
+ // The robot should not have gone anywhere.
+ EXPECT_NEAR(0.0, state_.squaredNorm(), 1e-25);
+}
+
+// Tests that, when we set the controller type to be line following, the target
+// selection freezes.
+TEST_F(LineFollowDrivetrainTest, FreezeOnControllerType) {
+ state_.setZero();
+ set_goal_pose({{0.0, 0.0, 0.0}, 0.0});
+ // Do one iteration to get the target into the drivetrain:
+ Iterate();
+
+ freeze_target_ = true;
+
+ // Set a goal pose that we should not go to.
+ set_goal_pose({{1.0, 1.0, 0.0}, M_PI_2});
+
+ RunForTime(::std::chrono::seconds(5));
+ EXPECT_NEAR(0.0, state_.squaredNorm(), 1e-25);
+}
+
+// Tests that when we freeze the controller without having acquired a target, we
+// don't do anything until a target arrives.
+TEST_F(LineFollowDrivetrainTest, FreezeWithoutAcquiringTarget) {
+ freeze_target_ = true;
+ target_selector_.set_has_target(false);
+ expect_output_ = false;
+ state_.setZero();
+
+ RunForTime(::std::chrono::seconds(5));
+
+ // Nothing should've happened
+ EXPECT_NEAR(0.0, state_.squaredNorm(), 1e-25);
+
+ // Now, provide a target:
+ target_selector_.set_has_target(true);
+ set_goal_pose({{1.0, 1.0, 0.0}, M_PI_2});
+ driver_model_ = [this](const ::Eigen::Matrix<double, 5, 1> &state) {
+ return (state.topRows<2>() - goal_pose().abs_pos().topRows<2>()).norm();
+ };
+ expect_output_ = true;
+
+ Iterate();
+
+ // And remove the target, to ensure that we keep going if we do loose the
+ // target:
+ target_selector_.set_has_target(false);
+
+ RunForTime(::std::chrono::seconds(15));
+
+ VerifyNearGoal();
+}
+
class LineFollowDrivetrainTargetParamTest
: public LineFollowDrivetrainTest,
public ::testing::WithParamInterface<Pose> {};
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 4da8b57..5d5b58d 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -3,11 +3,27 @@
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+#include "frc971/control_loops/pose.h"
namespace frc971 {
namespace control_loops {
namespace drivetrain {
+// An interface for target selection. This provides an object that will take in
+// state updates and then determine what poes we should be driving to.
+class TargetSelectorInterface {
+ public:
+ // Take the state as [x, y, theta, left_vel, right_vel]
+ // If unable to determine what target to go for, returns false. If a viable
+ // target is selected, then returns true and sets target_pose.
+ // TODO(james): Some implementations may also want a drivetrain goal so that
+ // driver intent can be divined more directly.
+ virtual bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state) = 0;
+ // Gets the current target pose. Should only be called if UpdateSelection has
+ // returned true.
+ virtual TypedPose<double> TargetPose() const = 0;
+};
+
// Defines an interface for classes that provide field-global localization.
class LocalizerInterface {
public:
@@ -34,6 +50,25 @@
virtual double right_velocity() const = 0;
virtual double left_voltage_error() const = 0;
virtual double right_voltage_error() const = 0;
+ virtual TargetSelectorInterface *target_selector() = 0;
+};
+
+// A target selector, primarily for testing purposes, that just lets a user
+// manually set the target selector state.
+class TrivialTargetSelector : public TargetSelectorInterface {
+ public:
+ bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &) override {
+ return has_target_;
+ }
+ TypedPose<double> TargetPose() const override { return pose_; }
+
+ void set_pose(const TypedPose<double> &pose) { pose_ = pose; }
+ void set_has_target(bool has_target) { has_target_ = has_target; }
+ bool has_target() const { return has_target_; }
+
+ private:
+ bool has_target_ = true;
+ TypedPose<double> pose_;
};
// Uses the generic HybridEkf implementation to provide a basic field estimator.
@@ -46,6 +81,7 @@
DeadReckonEkf(const DrivetrainConfig<double> &dt_config) : ekf_(dt_config) {
ekf_.ResetInitialState(::aos::monotonic_clock::now(), Ekf::State::Zero(),
ekf_.P());
+ target_selector_.set_has_target(false);
}
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
@@ -72,8 +108,13 @@
return ekf_.X_hat(StateIdx::kRightVoltageError);
}
+ TrivialTargetSelector *target_selector() override {
+ return &target_selector_;
+ }
+
private:
Ekf ekf_;
+ TrivialTargetSelector target_selector_;
};
} // namespace drivetrain