Merge "Send frames out on the queue"
diff --git a/aos/vision/blob/contour.h b/aos/vision/blob/contour.h
index 9cd1400..bbd59a9 100644
--- a/aos/vision/blob/contour.h
+++ b/aos/vision/blob/contour.h
@@ -25,6 +25,7 @@
ContourNode *pappend(int x, int y, AnalysisAllocator *alloc) {
return alloc->cons_obj<ContourNode>(x, y, this);
}
+ void set_point(Point new_pt) { pt = new_pt; }
Point pt;
ContourNode *next;
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index e332cfe..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",
],
)
@@ -101,6 +102,44 @@
)
cc_library(
+ name = "line_follow_drivetrain",
+ srcs = [
+ "line_follow_drivetrain.cc",
+ ],
+ hdrs = [
+ "line_follow_drivetrain.h",
+ ],
+ deps = [
+ ":drivetrain_config",
+ ":drivetrain_queue",
+ ":localizer",
+ "//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 +260,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..cab8043 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_, 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),
@@ -231,11 +232,9 @@
Y << position->left_encoder, position->right_encoder, last_gyro_rate_,
last_accel_;
kf_.Correct(Y);
- // TODO(james): Account for delayed_U as appropriate (should be
- // last_last_*_voltage).
- localizer_->Update({last_left_voltage_, last_right_voltage_}, monotonic_now,
- position->left_encoder, position->right_encoder,
- last_gyro_rate_, last_accel_);
+ localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
+ monotonic_now, position->left_encoder,
+ position->right_encoder, last_gyro_rate_, last_accel_);
}
dt_openloop_.SetPosition(position, left_gear_, right_gear_);
@@ -247,17 +246,22 @@
dt_closedloop_.SetGoal(*goal);
dt_openloop_.SetGoal(*goal);
dt_spline_.SetGoal(*goal);
+ dt_line_follow_.SetGoal(*goal);
}
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 +273,13 @@
case 2:
dt_spline_.SetOutput(output);
break;
+ case 3:
+ 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;
}
// The output should now contain the shift request.
@@ -310,6 +321,7 @@
dt_openloop_.PopulateStatus(status);
dt_closedloop_.PopulateStatus(status);
dt_spline_.PopulateStatus(status);
+ dt_line_follow_.PopulateStatus(status);
}
double left_voltage = 0.0;
@@ -334,6 +346,8 @@
// Gyro heading vs left-right
// Voltage error.
+ last_last_left_voltage_ = last_left_voltage_;
+ last_last_right_voltage_ = last_right_voltage_;
Eigen::Matrix<double, 2, 1> U;
U(0, 0) = last_left_voltage_;
U(1, 0) = last_right_voltage_;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 1aa3e13..800199b 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;
@@ -62,6 +64,9 @@
double last_left_voltage_ = 0;
double last_right_voltage_ = 0;
+ // The left/right voltages previous to last_*_voltage_.
+ double last_last_left_voltage_ = 0;
+ double last_last_right_voltage_ = 0;
bool left_high_requested_;
bool right_high_requested_;
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..97aaba5 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -719,6 +719,42 @@
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) {
+ 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;
+ goal->throttle = 0.5;
+ goal.Send();
+
+ RunForTime(chrono::seconds(5));
+ // 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);
+}
+
::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/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index f931e59..1d7aa0b 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -197,9 +197,9 @@
void TrajectoryGetPlanXVA(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec,
double *X, double *V, double *A) {
for (size_t i = 0; i < vec->size(); ++i) {
- X[i] = (*vec)[0][0];
- V[i] = (*vec)[0][1];
- A[i] = (*vec)[0][2];
+ X[i] = (*vec)[i][0];
+ V[i] = (*vec)[i][1];
+ A[i] = (*vec)[i][2];
}
}
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..c4ca5ef
--- /dev/null
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -0,0 +1,238 @@
+#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,
+ 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_)),
+ target_selector_(target_selector),
+ U_(0, 0) {}
+
+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) {
+ // TODO(james): More properly calculate goal velocity from throttle.
+ goal_velocity_ = goal.throttle;
+ // 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;
+}
+
+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 && 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_);
+ // 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..83fba20
--- /dev/null
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -0,0 +1,101 @@
+#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"
+#include "frc971/control_loops/drivetrain/localizer.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,
+ 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);
+ // State: [x, y, theta, left_vel, right_vel]
+ void Update(const ::Eigen::Matrix<double, 5, 1> &state);
+ // 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(
+ ::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_;
+
+ TargetSelectorInterface *target_selector_;
+ bool freeze_target_ = false;
+ bool have_target_ = false;
+ 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..65cf9bf
--- /dev/null
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -0,0 +1,368 @@
+#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_, &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) { target_selector_.set_pose(pose); }
+
+ Pose goal_pose() const { return target_selector_.TargetPose(); }
+
+ 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_);
+ goal.controller_type = freeze_target_ ? 3 : 0;
+ drivetrain_.SetGoal(goal);
+ drivetrain_.Update(state_);
+
+ ::frc971::control_loops::DrivetrainQueue::Output 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);
+
+ ::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();
+
+ 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(), target.abs_pos().x()});
+ ::std::vector<double> line_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_,
+ {{"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_ = ::Eigen::Matrix<double, 5, 1>::Zero();
+ TrivialTargetSelector target_selector_;
+
+ bool freeze_target_ = false;
+ bool expect_output_ = true;
+
+ 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_;
+
+ 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);
+}
+
+// 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> {};
+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
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
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 4656cb3..981701a 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -35,6 +35,7 @@
Localizer::State::Zero(), localizer_.P());
frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>(
".y2019.control_loops.drivetrain.camera_frames");
+ target_selector_.set_has_target(false);
}
void EventLoopLocalizer::Reset(const Localizer::State &state) {
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 2812108..eb03627 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -58,6 +58,11 @@
return localizer_.X_hat(StateIdx::kRightVoltageError);
}
+ ::frc971::control_loops::drivetrain::TrivialTargetSelector *target_selector()
+ override {
+ return &target_selector_;
+ }
+
private:
void HandleFrame(const CameraFrame &frame);
@@ -71,6 +76,7 @@
Pose robot_pose_;
const ::std::array<Camera, constants::Values::kNumCameras> cameras_;
Localizer localizer_;
+ ::frc971::control_loops::drivetrain::TrivialTargetSelector target_selector_;
};
// Constructs the cameras based on the constants in the //y2019/constants.*
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index bcbad52..d491c14 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -76,7 +76,6 @@
void RunIteration() {
drivetrain_motor_plant_.SendPositionMessage();
drivetrain_motor_.Iterate();
- drivetrain_motor_plant_.Simulate();
if (enable_cameras_) {
SendDelayedFrames();
if (last_frame_ + ::std::chrono::milliseconds(33) <
@@ -85,6 +84,7 @@
last_frame_ = monotonic_clock::now();
}
}
+ drivetrain_motor_plant_.Simulate();
SimulateTimestep(true, dt_config_.dt);
}
@@ -107,14 +107,14 @@
drivetrain_motor_plant_.GetRightPosition(), 1e-3);
}
- void VerifyEstimatorAccurate() {
+ void VerifyEstimatorAccurate(double eps) {
const Eigen::Matrix<double, 5, 1> true_state =
drivetrain_motor_plant_.state();
- EXPECT_NEAR(localizer_.x(), true_state(0, 0), 1e-5);
- EXPECT_NEAR(localizer_.y(), true_state(1, 0), 1e-5);
- EXPECT_NEAR(localizer_.theta(), true_state(2, 0), 1e-5);
- EXPECT_NEAR(localizer_.left_velocity(), true_state(3, 0), 1e-5);
- EXPECT_NEAR(localizer_.right_velocity(), true_state(4, 0), 1e-5);
+ EXPECT_NEAR(localizer_.x(), true_state(0, 0), eps);
+ EXPECT_NEAR(localizer_.y(), true_state(1, 0), eps);
+ EXPECT_NEAR(localizer_.theta(), true_state(2, 0), eps);
+ EXPECT_NEAR(localizer_.left_velocity(), true_state(3, 0), eps);
+ EXPECT_NEAR(localizer_.right_velocity(), true_state(4, 0), eps);
}
void CaptureFrames() {
@@ -199,7 +199,20 @@
.Send();
RunForTime(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate();
+ VerifyEstimatorAccurate(1e-10);
+}
+
+// Tests that camera udpates with a perfect models results in no errors.
+TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdate) {
+ set_enable_cameras(true);
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .controller_type(1)
+ .left_goal(-1.0)
+ .right_goal(1.0)
+ .Send();
+ RunForTime(chrono::seconds(3));
+ VerifyNearGoal();
+ VerifyEstimatorAccurate(1e-8);
}
// Tests that not having cameras with an initial disturbance results in
@@ -237,7 +250,7 @@
.Send();
RunForTime(chrono::seconds(3));
VerifyNearGoal();
- VerifyEstimatorAccurate();
+ VerifyEstimatorAccurate(1e-5);
}
} // namespace testing
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 40c9027..9290a2f 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -71,23 +71,23 @@
const ElevatorWristPosition kPanelForwardLowerPos{0.0, M_PI / 2.0};
const ElevatorWristPosition kPanelBackwardLowerPos{0.0, -M_PI / 2.0};
-const ElevatorWristPosition kPanelForwardMiddlePos{0.7412, M_PI / 2.0};
-const ElevatorWristPosition kPanelBackwardMiddlePos{0.7412, -M_PI / 2.0};
+const ElevatorWristPosition kPanelForwardMiddlePos{0.75, M_PI / 2.0};
+const ElevatorWristPosition kPanelBackwardMiddlePos{0.78, -M_PI / 2.0};
-const ElevatorWristPosition kPanelForwardUpperPos{1.4524, M_PI / 2.0};
-const ElevatorWristPosition kPanelBackwardUpperPos{1.4524, -M_PI / 2.0};
+const ElevatorWristPosition kPanelForwardUpperPos{1.51, M_PI / 2.0};
+const ElevatorWristPosition kPanelBackwardUpperPos{1.50, -M_PI / 2.0};
-const ElevatorWristPosition kBallForwardLowerPos{0.0, 0.98};
-const ElevatorWristPosition kBallBackwardLowerPos{0.607, -2.281};
+const ElevatorWristPosition kBallForwardLowerPos{0.4598, 1.5863};
+const ElevatorWristPosition kBallBackwardLowerPos{0.175, -1.61};
-const ElevatorWristPosition kBallForwardMiddlePos{0.67945, 1.22};
-const ElevatorWristPosition kBallBackwardMiddlePos{0.93345, -1.83};
+const ElevatorWristPosition kBallForwardMiddlePos{1.16, 1.546};
+const ElevatorWristPosition kBallBackwardMiddlePos{0.876021, -1.546};
-const ElevatorWristPosition kBallForwardUpperPos{1.41605, 1.22};
-const ElevatorWristPosition kBallBackwardUpperPos{1.41605, -1.36};
+const ElevatorWristPosition kBallForwardUpperPos{1.50, 0.961};
+const ElevatorWristPosition kBallBackwardUpperPos{1.41, -1.217};
-const ElevatorWristPosition kBallCargoForwardPos{0.73025, M_PI / 2};
-const ElevatorWristPosition kBallCargoBackwardPos{0.80645, -1.92};
+const ElevatorWristPosition kBallCargoForwardPos{0.699044, 1.353};
+const ElevatorWristPosition kBallCargoBackwardPos{0.828265, -1.999};
const ElevatorWristPosition kBallHPIntakeForwardPos{0.340, 0.737};
const ElevatorWristPosition kBallHPIntakeBackwardPos{0.52, -1.1};
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index 7300850..0221254 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -11,8 +11,8 @@
cc_library(
name = "constants",
- hdrs = ["constants.h"],
srcs = ["constants.cc"],
+ hdrs = ["constants.h"],
)
cc_library(
@@ -70,40 +70,53 @@
"//y2019/jevois:serial",
"//y2019/jevois:structures",
"//y2019/jevois:uart",
- "//y2019/jevois/camera:reader",
"//y2019/jevois/camera:image_stream",
+ "//y2019/jevois/camera:reader",
"@com_google_ceres_solver//:ceres",
],
)
cc_binary(
+ name = "serial_waiter",
+ srcs = ["serial_waiter.cc"],
+ restricted_to = VISION_TARGETS,
+ deps = [
+ "//aos/time",
+ "//y2019/jevois:serial",
+ ],
+)
+
+cc_binary(
name = "debug_serial",
srcs = ["debug_serial.cc"],
deps = [
+ "//aos/logging",
+ "//aos/logging:implementations",
"//y2019/jevois:serial",
"//y2019/jevois:structures",
"//y2019/jevois:uart",
- "//aos/logging",
- "//aos/logging:implementations",
],
)
cc_binary(
name = "global_calibration",
- srcs = ["global_calibration.cc", "constants_formatting.cc"],
- deps = [
- ":target_finder",
- "//aos/logging",
- "//aos/logging:implementations",
- "//aos/vision/blob:find_blob",
- "//aos/vision/blob:codec",
- "//aos/vision/events:epoll_events",
- "//aos/vision/events:socket_types",
- "//aos/vision/events:udp",
- "//aos/vision/image:image_stream",
- "//aos/vision/image:image_dataset",
- "//aos/vision/image:reader",
- "@com_google_ceres_solver//:ceres",
+ srcs = [
+ "constants_formatting.cc",
+ "global_calibration.cc",
],
restricted_to = VISION_TARGETS,
+ deps = [
+ ":target_finder",
+ "//aos/logging",
+ "//aos/logging:implementations",
+ "//aos/vision/blob:codec",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:udp",
+ "//aos/vision/image:image_dataset",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:reader",
+ "@com_google_ceres_solver//:ceres",
+ ],
)
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 4f43c9a..533dcf6 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -92,10 +92,23 @@
// Find polygons from blobs.
std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
+ // Convert blobs to contours in the corrected space.
+ ContourNode* contour = finder_.GetContour(blob);
+ if (draw_contours_) {
+ DrawContour(contour, {255, 0, 0});
+ }
+ finder_.UnWarpContour(contour);
+ if (draw_contours_) {
+ DrawContour(contour, {0, 0, 255});
+ }
+
+ // Process to polygons.
std::vector<Segment<2>> polygon =
- finder_.FillPolygon(blob, draw_raw_poly_);
+ finder_.FillPolygon(contour, draw_raw_poly_);
if (polygon.empty()) {
- DrawBlob(blob, {255, 0, 0});
+ if (!draw_contours_) {
+ DrawBlob(blob, {255, 0, 0});
+ }
} else {
raw_polys.push_back(polygon);
if (draw_select_blob_) {
@@ -168,7 +181,21 @@
} else if (key == 'b') {
draw_raw_poly_ = !draw_raw_poly_;
} else if (key == 'n') {
+ draw_contours_ = !draw_contours_;
+ } else if (key == 'm') {
draw_select_blob_ = !draw_select_blob_;
+ } else if (key == 'h') {
+ printf("Key Mappings:\n");
+ printf(" z: Toggle drawing final target pose.\n");
+ printf(" x: Toggle drawing re-projected targets and print solver results.\n");
+ printf(" c: Toggle drawing proposed target groupings.\n");
+ printf(" v: Toggle drawing ordered target components.\n");
+ printf(" b: Toggle drawing proposed target components.\n");
+ printf(" n: Toggle drawing countours before and after warping.\n");
+ printf(" m: Toggle drawing raw blob data (may need to change image to toggle a redraw).\n");
+ printf(" h: Print this message.\n");
+ printf(" a: May log camera image to /tmp/debug_viewer_jpeg_<#>.yuyv"
+ printf(" q: Exit the application.\n");
} else if (key == 'q') {
printf("User requested shutdown.\n");
exit(0);
@@ -178,6 +205,17 @@
};
}
+ void DrawContour(ContourNode *contour, PixelRef color) {
+ if (viewer_) {
+ for (ContourNode *node = contour; node->next != contour;) {
+ Vector<2> a(node->pt.x, node->pt.y);
+ Vector<2> b(node->next->pt.x, node->next->pt.y);
+ overlay_.AddLine(a, b, color);
+ node = node->next;
+ }
+ }
+ }
+
void DrawComponent(const TargetComponent &comp, PixelRef top_color,
PixelRef bot_color, PixelRef in_color,
PixelRef out_color) {
@@ -252,6 +290,7 @@
BlobList imgs_last_;
ImageFormat fmt_last_;
bool draw_select_blob_ = false;
+ bool draw_contours_ = false;
bool draw_raw_poly_ = false;
bool draw_components_ = false;
bool draw_raw_target_ = false;
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index d5f0d4e..bf64310 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -133,7 +133,10 @@
bool verbose = false;
std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
- std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
+ // Convert blobs to contours in the corrected space.
+ ContourNode* contour = finder_.GetContour(blob);
+ finder_.UnWarpContour(contour);
+ std::vector<Segment<2>> polygon = finder_.FillPolygon(contour, verbose);
if (polygon.empty()) {
} else {
raw_polys.push_back(polygon);
diff --git a/y2019/vision/serial_waiter.cc b/y2019/vision/serial_waiter.cc
new file mode 100644
index 0000000..551e179
--- /dev/null
+++ b/y2019/vision/serial_waiter.cc
@@ -0,0 +1,48 @@
+#include <unistd.h>
+
+#include <chrono>
+
+#include "y2019/jevois/serial.h"
+#include "aos/time/time.h"
+
+using ::aos::monotonic_clock;
+using ::y2019::jevois::open_via_terminos;
+
+namespace chrono = ::std::chrono;
+
+int main(int /*argc*/, char ** /*argv*/) {
+ int serial_fd = open_via_terminos("/dev/ttyS0");
+
+ // Print out a startup message. The Teensy will ignore it as a corrupt
+ // packet, but it gives us some warm fuzzies that things are booting right.
+ (void)write(
+ serial_fd,
+ "Starting target_sender in 3 seconds. Press 10 a's to interrupt.\r\n",
+ 66);
+
+ // Give the user 3 seconds to press 10 a's. If they don't do it in time,
+ // return 0 to signal that we should boot, or 1 that we are asked not to boot.
+ constexpr int kACount = 10;
+ int a_count = 0;
+ const monotonic_clock::time_point end_time =
+ monotonic_clock::now() + chrono::seconds(3);
+ do {
+ constexpr size_t kBufferSize = 16;
+ char data[kBufferSize];
+ ssize_t n = ::read(serial_fd, &data[0], kBufferSize);
+ for (int i = 0; i < n; ++i) {
+ if (data[i] == 'a') {
+ ++a_count;
+ if (a_count > kACount) {
+ ::close(serial_fd);
+ return 1;
+ }
+ } else {
+ a_count = 0;
+ }
+ }
+ } while (monotonic_clock::now() < end_time);
+
+ ::close(serial_fd);
+ return 0;
+}
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index 6b90080..b46d802 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -33,13 +33,64 @@
imgs->end());
}
+ContourNode* TargetFinder::GetContour(const RangeImage &blob) {
+ alloc_.reset();
+ return RangeImgToContour(blob, &alloc_);
+}
+
+// TODO(ben): These values will be moved into the constants.h file.
+namespace {
+
+constexpr double f_x = 481.4957;
+constexpr double c_x = 341.215;
+constexpr double f_y = 484.314;
+constexpr double c_y = 251.29;
+
+constexpr double f_x_prime = 363.1424;
+constexpr double c_x_prime = 337.9895;
+constexpr double f_y_prime = 366.4837;
+constexpr double c_y_prime = 240.0702;
+
+constexpr double k_1 = -0.2739;
+constexpr double k_2 = 0.01583;
+constexpr double k_3 = 0.04201;
+
+constexpr int iterations = 7;
+
+}
+
+Point UnWarpPoint(const Point &point, int iterations) {
+ const double x0 = ((double)point.x - c_x) / f_x;
+ const double y0 = ((double)point.y - c_y) / f_y;
+ double x = x0;
+ double y = y0;
+ for (int i = 0; i < iterations; i++) {
+ const double r_sqr = x * x + y * y;
+ const double coeff =
+ 1.0 + r_sqr * (k_1 + k_2 * r_sqr * (1.0 + k_3 * r_sqr));
+ x = x0 / coeff;
+ y = y0 / coeff;
+ }
+ double nx = x * f_x_prime + c_x_prime;
+ double ny = y * f_y_prime + c_y_prime;
+ Point p = {static_cast<int>(nx), static_cast<int>(ny)};
+ return p;
+}
+
+void TargetFinder::UnWarpContour(ContourNode *start) const {
+ ContourNode *node = start;
+ while (node->next != start) {
+ node->set_point(UnWarpPoint(node->pt, iterations));
+ node = node->next;
+ }
+ node->set_point(UnWarpPoint(node->pt, iterations));
+}
+
// TODO: Try hierarchical merge for this.
// Convert blobs into polygons.
std::vector<aos::vision::Segment<2>> TargetFinder::FillPolygon(
- const RangeImage &blob, bool verbose) {
+ ContourNode* start, bool verbose) {
if (verbose) printf("Process Polygon.\n");
- alloc_.reset();
- ContourNode *start = RangeImgToContour(blob, &alloc_);
struct Pt {
float x;
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index 5957fa6..bdf67f2 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -4,6 +4,7 @@
#include "aos/vision/blob/region_alloc.h"
#include "aos/vision/blob/threshold.h"
#include "aos/vision/blob/transpose.h"
+#include "aos/vision/blob/contour.h"
#include "aos/vision/debug/overlay.h"
#include "aos/vision/math/vector.h"
#include "y2019/vision/target_types.h"
@@ -15,6 +16,7 @@
using aos::vision::RangeImage;
using aos::vision::BlobList;
using aos::vision::Vector;
+using aos::vision::ContourNode;
class TargetFinder {
public:
@@ -28,8 +30,11 @@
// filter out obvious or durranged blobs.
void PreFilter(BlobList *imgs);
+ ContourNode* GetContour(const RangeImage &blob);
+ void UnWarpContour(ContourNode* start) const;
+
// Turn a blob into a polgygon.
- std::vector<aos::vision::Segment<2>> FillPolygon(const RangeImage &blob,
+ std::vector<aos::vision::Segment<2>> FillPolygon(ContourNode *start,
bool verbose);
// Turn a bloblist into components of a target.
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 5f2afb9..4c14969 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -309,9 +309,11 @@
bool verbose = false;
std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
- std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
- if (polygon.empty()) {
- } else {
+ // Convert blobs to contours in the corrected space.
+ ContourNode* contour = finder_.GetContour(blob);
+ finder_.UnWarpContour(contour);
+ std::vector<Segment<2>> polygon = finder_.FillPolygon(contour, verbose);
+ if (!polygon.empty()) {
raw_polys.push_back(polygon);
}
}
diff --git a/y2019/vision/tools/deploy.sh b/y2019/vision/tools/deploy.sh
index 4ad926f..ff3ee01 100755
--- a/y2019/vision/tools/deploy.sh
+++ b/y2019/vision/tools/deploy.sh
@@ -1,4 +1,12 @@
#!/bin/sh
+echo "Building executables"
+readonly BAZEL_OPTIONS="-c opt --cpu=armhf-debian"
+readonly BAZEL_BIN="$(bazel info ${BAZEL_OPTIONS} bazel-bin)"
+
+bazel build ${BAZEL_OPTIONS} \
+ //y2019/vision:target_sender \
+ //y2019/vision:serial_waiter
+
echo "Mount jevois ..."
./jevois-cmd usbsd
@@ -11,6 +19,11 @@
echo "Copying files ..."
cp ./austin_cam.sh /media/$USER/JEVOIS/
+cp ./launch.sh /media/$USER/JEVOIS/deploy/
+
+cp "${BAZEL_BIN}/y2019/vision/target_sender" \
+ "${BAZEL_BIN}/y2019/vision/serial_waiter" \
+ /media/$USER/JEVOIS/deploy/
echo "Unmount sd card ..."
umount /media/$USER/JEVOIS
diff --git a/y2019/vision/tools/launch.sh b/y2019/vision/tools/launch.sh
index f2bd68d..b23fe2c 100755
--- a/y2019/vision/tools/launch.sh
+++ b/y2019/vision/tools/launch.sh
@@ -1 +1,5 @@
+if /jevois/deploy/serial_waiter; then
+ /jevois/deploy/target_sender &> /jevois/log.log
+fi
+
touch /tmp/do_not_export_sd_card