Fix edge case in coerce and add tests
This adds a baseline test case, as well as two tests (horizontal &
vertical) which failed prior to this change. I would encourage someone
to write more tests for the coerce goal functionality, but I am not
going to do it in this change :P
Change-Id: I76ee32cec46b9557fc5f5a09fa172be64b82cc70
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 17613ab..7d3b915 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -142,6 +142,22 @@
],
)
+cc_test(
+ name = "coerce_goal_test",
+ srcs = [
+ "coerce_goal_test.cc",
+ ],
+ linkopts = [
+ "-lm",
+ ],
+ deps = [
+ ":coerce_goal",
+ "//aos/controls:polytope",
+ "//aos/testing:googletest",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
# TODO(austin): Select isn't working right. We should be able to remove
# logging conditionally with select and have CPU constraints work correctly.
cc_library(
diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index 311d882..6fdc70f 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -1,11 +1,8 @@
#include "frc971/control_loops/coerce_goal.h"
#include "Eigen/Dense"
-
#include "aos/controls/polytope.h"
namespace frc971 {
-namespace control_loops {
-
-} // namespace control_loops
+namespace control_loops {} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 6e927b0..f9a52cc 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -50,7 +50,7 @@
if (projectedh(i, 0) > 0) {
max_boundary =
::std::min(max_boundary, projectedk(i, 0) / projectedh(i, 0));
- } else {
+ } else if (projectedh(i, 0) != 0) {
min_boundary =
::std::max(min_boundary, projectedk(i, 0) / projectedh(i, 0));
}
diff --git a/frc971/control_loops/coerce_goal_test.cc b/frc971/control_loops/coerce_goal_test.cc
new file mode 100644
index 0000000..11a0de9
--- /dev/null
+++ b/frc971/control_loops/coerce_goal_test.cc
@@ -0,0 +1,72 @@
+#include "frc971/control_loops/coerce_goal.h"
+
+#include <unistd.h>
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace {
+
+::aos::controls::HVPolytope<2, 4, 4, double> poly() {
+ return ::aos::controls::HVPolytope<2, 4, 4, double>(
+ (Eigen::Matrix<double, 4, 2>() << /*[[*/ 1, 0 /*]*/,
+ /*[*/ -1, 0 /*]*/,
+ /*[*/ 0, 1 /*]*/,
+ /*[*/ 0, -1 /*]]*/)
+ .finished(),
+ (Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
+ /*[*/ 12 /*]*/,
+ /*[*/ 12 /*]*/,
+ /*[*/ 12 /*]*/)
+ .finished(),
+ (Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
+ /*[*/ -12, 12, 12, -12 /*]*/)
+ .finished());
+}
+
+TEST(CoerceGoalTest, WithinRegion) {
+ const auto upoly = poly();
+ Eigen::Matrix<double, 1, 2> k;
+ k << 2, 2;
+
+ Eigen::Matrix<double, 2, 1> goal;
+ goal << -2, 2;
+
+ auto result = CoerceGoal<double>(upoly, k, 0, goal);
+
+ EXPECT_EQ(result(0, 0), -2);
+ EXPECT_EQ(result(1, 0), 2);
+}
+
+TEST(CoerceGoalTest, VerticalLine) {
+ const auto upoly = poly();
+ Eigen::Matrix<double, 1, 2> k;
+ k << 2, 0;
+
+ Eigen::Matrix<double, 2, 1> goal;
+ goal << 0, 13;
+
+ auto result = CoerceGoal<double>(upoly, k, 0, goal);
+
+ EXPECT_EQ(result(0, 0), 0);
+ EXPECT_EQ(result(1, 0), 12);
+}
+
+TEST(CoerceGoalTest, HorizontalLine) {
+ const auto upoly = poly();
+ Eigen::Matrix<double, 1, 2> k;
+ k << 0, 2;
+
+ Eigen::Matrix<double, 2, 1> goal;
+ goal << 13, 2;
+
+ auto result = CoerceGoal<double>(upoly, k, 0, goal);
+
+ EXPECT_EQ(result(0, 0), 12);
+ EXPECT_EQ(result(1, 0), 0);
+}
+
+} // namespace
+} // namespace control_loops
+} // namespace frc971