Re-enable polytopes for closed loop driving
I reproduced the original bug which resulted in this being disabled in a
test and fixed it already (I99a1299e538a395685c2e7e555dfccab48055349).
This commit fixes another issue observed in testing and re-enables the
polytopes.
Change-Id: Ic27e0ab8b2ea6dd480d7998c44530918443874db
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 9db00b5..9d7e223 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -224,6 +224,48 @@
}
}
+// Tests that the robot successfully drives straight forward.
+// This used to not work due to a U-capping bug.
+TEST_F(DrivetrainTest, DriveStraightForward) {
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .left_goal(4.0)
+ .right_goal(4.0)
+ .Send();
+ for (int i = 0; i < 500; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ EXPECT_FLOAT_EQ(my_drivetrain_queue_.output->left_voltage,
+ my_drivetrain_queue_.output->right_voltage);
+ EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -3);
+ EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -3);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that the robot successfully drives close to straight.
+// This used to fail in simulation due to libcdd issues with U-capping.
+TEST_F(DrivetrainTest, DriveAlmostStraightForward) {
+ my_drivetrain_queue_.goal.MakeWithBuilder()
+ .control_loop_driving(true)
+ .left_goal(4.0)
+ .right_goal(3.9)
+ .Send();
+ for (int i = 0; i < 500; ++i) {
+ drivetrain_motor_plant_.SendPositionMessage();
+ drivetrain_motor_.Iterate();
+ drivetrain_motor_plant_.Simulate();
+ SimulateTimestep(true);
+ ASSERT_TRUE(my_drivetrain_queue_.output.FetchLatest());
+ EXPECT_GT(my_drivetrain_queue_.output->left_voltage, -3);
+ EXPECT_GT(my_drivetrain_queue_.output->right_voltage, -3);
+ }
+ VerifyNearGoal();
+}
+
::aos::controls::HPolytope<2> 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/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 049b576..9fdfe1c 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -18,98 +18,95 @@
DrivetrainMotorsSS::LimitedDrivetrainLoop::LimitedDrivetrainLoop(
StateFeedbackLoop<4, 2, 2> &&loop)
: StateFeedbackLoop<4, 2, 2>(::std::move(loop)),
- U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
+ U_poly_((Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1)
.finished(),
(Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0)
.finished()) {
::aos::controls::HPolytope<0>::Init();
- T << 1, -1, 1, 1;
- T_inverse = T.inverse();
+ T_ << 1, 1, 1, -1;
+ T_inverse_ = T_.inverse();
}
+// This intentionally runs the U-capping code even when it's unnecessary to help
+// make it more deterministic. Only running it when one or both sides want
+// out-of-range voltages could lead to things like running out of CPU under
+// certain situations, which would be bad.
void DrivetrainMotorsSS::LimitedDrivetrainLoop::CapU() {
+ output_was_capped_ = ::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0;
+
const Eigen::Matrix<double, 4, 1> error = R() - X_hat();
- if (::std::abs(U(0, 0)) > 12.0 || ::std::abs(U(1, 0)) > 12.0) {
- mutable_U() =
- U() * 12.0 / ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0)));
- LOG_MATRIX(DEBUG, "U is now", U());
- // TODO(Austin): Figure out why the polytope stuff wasn't working and
- // remove this hack.
- output_was_capped_ = true;
- return;
+ LOG_MATRIX(DEBUG, "U at start", U());
+ LOG_MATRIX(DEBUG, "R at start", R());
+ LOG_MATRIX(DEBUG, "Xhat at start", X_hat());
- LOG_MATRIX(DEBUG, "U at start", U());
- LOG_MATRIX(DEBUG, "R at start", R());
- LOG_MATRIX(DEBUG, "Xhat at start", X_hat());
+ Eigen::Matrix<double, 2, 2> position_K;
+ position_K << K(0, 0), K(0, 2), K(1, 0), K(1, 2);
+ Eigen::Matrix<double, 2, 2> velocity_K;
+ velocity_K << K(0, 1), K(0, 3), K(1, 1), K(1, 3);
- Eigen::Matrix<double, 2, 2> position_K;
- position_K << K(0, 0), K(0, 2), K(1, 0), K(1, 2);
- Eigen::Matrix<double, 2, 2> velocity_K;
- velocity_K << K(0, 1), K(0, 3), K(1, 1), K(1, 3);
+ Eigen::Matrix<double, 2, 1> position_error;
+ position_error << error(0, 0), error(2, 0);
+ // drive_error = [total_distance_error, left_error - right_error]
+ const auto drive_error = T_inverse_ * position_error;
+ Eigen::Matrix<double, 2, 1> velocity_error;
+ velocity_error << error(1, 0), error(3, 0);
+ LOG_MATRIX(DEBUG, "error", error);
- Eigen::Matrix<double, 2, 1> position_error;
- position_error << error(0, 0), error(2, 0);
- const auto drive_error = T_inverse * position_error;
- Eigen::Matrix<double, 2, 1> velocity_error;
- velocity_error << error(1, 0), error(3, 0);
- LOG_MATRIX(DEBUG, "error", error);
+ const ::aos::controls::HPolytope<2> pos_poly(U_poly_, position_K * T_,
+ -velocity_K * velocity_error);
- const auto &poly = U_Poly_;
- const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K * T;
- const Eigen::Matrix<double, 4, 1> pos_poly_k =
- poly.k() - poly.H() * velocity_K * velocity_error;
- const ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+ Eigen::Matrix<double, 2, 1> adjusted_pos_error;
+ {
+ const auto &P = drive_error;
- Eigen::Matrix<double, 2, 1> adjusted_pos_error;
- {
- const auto &P = drive_error;
+ Eigen::Matrix<double, 1, 2> L45;
+ L45 << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
+ const double w45 = 0;
- Eigen::Matrix<double, 1, 2> L45;
- L45 << ::aos::sign(P(1, 0)), -::aos::sign(P(0, 0));
- const double w45 = 0;
+ Eigen::Matrix<double, 1, 2> LH;
+ if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
+ LH << 0, 1;
+ } else {
+ LH << 1, 0;
+ }
+ const double wh = LH.dot(P);
- Eigen::Matrix<double, 1, 2> LH;
- if (::std::abs(P(0, 0)) > ::std::abs(P(1, 0))) {
- LH << 0, 1;
- } else {
- LH << 1, 0;
- }
- const double wh = LH.dot(P);
+ Eigen::Matrix<double, 2, 2> standard;
+ standard << L45, LH;
+ Eigen::Matrix<double, 2, 1> W;
+ W << w45, wh;
+ const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
- Eigen::Matrix<double, 2, 2> standard;
- standard << L45, LH;
- Eigen::Matrix<double, 2, 1> W;
- W << w45, wh;
- const Eigen::Matrix<double, 2, 1> intersection = standard.inverse() * W;
-
- bool is_inside_h;
- const auto adjusted_pos_error_h =
- DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
- const auto adjusted_pos_error_45 =
- DoCoerceGoal(pos_poly, L45, w45, intersection, nullptr);
- if (pos_poly.IsInside(intersection)) {
- adjusted_pos_error = adjusted_pos_error_h;
- } else {
- if (is_inside_h) {
- if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm() ||
- adjusted_pos_error_45.norm() > intersection.norm()) {
- adjusted_pos_error = adjusted_pos_error_h;
- } else {
- adjusted_pos_error = adjusted_pos_error_45;
- }
+ bool is_inside_h, is_inside_45;
+ const auto adjusted_pos_error_h =
+ DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
+ const auto adjusted_pos_error_45 =
+ DoCoerceGoal(pos_poly, L45, w45, intersection, &is_inside_45);
+ if (pos_poly.IsInside(intersection)) {
+ adjusted_pos_error = adjusted_pos_error_h;
+ } else {
+ if (is_inside_h) {
+ if (adjusted_pos_error_h.norm() > adjusted_pos_error_45.norm() ||
+ adjusted_pos_error_45.norm() > intersection.norm()) {
+ adjusted_pos_error = adjusted_pos_error_h;
} else {
adjusted_pos_error = adjusted_pos_error_45;
}
+ } else {
+ adjusted_pos_error = adjusted_pos_error_45;
}
}
+ }
- LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
- mutable_U() =
- velocity_K * velocity_error + position_K * T * adjusted_pos_error;
- LOG_MATRIX(DEBUG, "U is now", U());
- } else {
- output_was_capped_ = false;
+ mutable_U() =
+ velocity_K * velocity_error + position_K * T_ * adjusted_pos_error;
+ LOG_MATRIX(DEBUG, "U is now", U());
+
+ if (!output_was_capped_) {
+ if ((U() - U_uncapped()).norm() > 0.0001) {
+ LOG(FATAL, "U unnecessarily capped\n");
+ }
}
}
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 1ed3eb7..193dbb2 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -27,9 +27,15 @@
private:
void CapU() override;
- const ::aos::controls::HPolytope<2> U_Poly_;
- Eigen::Matrix<double, 2, 2> T, T_inverse;
- bool output_was_capped_ = false;;
+ // Reprsents +/- full power on each motor in U-space, aka the square from
+ // (-12, -12) to (12, 12).
+ const ::aos::controls::HPolytope<2> U_poly_;
+
+ // multiplying by T converts [left_error, right_error] to
+ // [left_right_error_difference, total_distance_error].
+ Eigen::Matrix<double, 2, 2> T_, T_inverse_;
+
+ bool output_was_capped_ = false;
};
DrivetrainMotorsSS(const DrivetrainConfig &dt_config);