Fix another couple of bugs in CoerceGoal
-We weren't properly handling the case where the line was outside of the
box.
-We weren't properly handling the case where w != 0.
-Rearrange the tests.
-Write some new tests.
Change-Id: Ib76b4e1670727d567c943f730c91d9536eb67ea7
diff --git a/aos/controls/polytope.h b/aos/controls/polytope.h
index c63166f..28f011e 100644
--- a/aos/controls/polytope.h
+++ b/aos/controls/polytope.h
@@ -109,13 +109,15 @@
#ifdef __linux__
+
template <int number_of_dimensions>
class HPolytope : public Polytope<number_of_dimensions> {
public:
// Constructs a polytope given the H and k matrices.
- HPolytope(Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic,
- number_of_dimensions>> H,
- Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k)
+ HPolytope(
+ Eigen::Ref<
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> H,
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k)
: H_(H), k_(k), vertices_(CalculateVertices(H, k)) {}
// This is an initialization function shared across all instantiations of this
@@ -133,6 +135,8 @@
return k_;
}
+ // NOTE: If you are getting bizarre errors that you are tracing back to the
+ // vertex matrix being funky, please check that you correctly called Init().
Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
const override {
return vertices_;
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index f9a52cc..b682b85 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -35,14 +35,34 @@
if (is_inside) *is_inside = true;
return R;
}
+ const Scalar norm_K = K.norm();
Eigen::Matrix<Scalar, 2, 1> parallel_vector;
Eigen::Matrix<Scalar, 2, 1> perpendicular_vector;
- perpendicular_vector = K.transpose().normalized();
+ // Calculate a vector that is perpendicular to the line defined by K * x = w.
+ perpendicular_vector = K.transpose() / norm_K;
+ // Calculate a vector that is parallel to the line defined by K * x = w.
parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
- Eigen::Matrix<Scalar, 4, 1> projectedh = region.static_H() * parallel_vector;
- Eigen::Matrix<Scalar, 4, 1> projectedk =
- region.static_k() - region.static_H() * perpendicular_vector * w;
+ // Calculate the location along the K x = w line where each boundary of the
+ // polytope would intersect.
+ // I.e., we want to calculate the distance along the K x = w line, as
+ // represented by
+ // parallel_vector * dist + perpendicular_vector * w / norm(K),
+ // such that it intersects with H_i * x = k_i.
+ // This gives us H_i * (parallel * dist + perp * w / norm(K)) = k_i.
+ // dist = (k_i - H_i * perp * w / norm(K)) / (H_i * parallel)
+ // projectedh is the numerator, projectedk is the denominator.
+ // The case where H_i * parallel is zero indicates a scenario where the given
+ // boundary of the polytope is parallel to the line, and so there is no
+ // meaningful value of dist to return.
+ // Note that the sign of projectedh will also indicate whether the distance is
+ // an upper or lower bound. If since valid points satisfy H_i * x < k_i, then
+ // if H_i * parallel is less than zero, then dist will be a lower bound and
+ // if it is greater than zero, then dist will be an upper bound.
+ const Eigen::Matrix<Scalar, 4, 1> projectedh =
+ region.static_H() * parallel_vector;
+ const Eigen::Matrix<Scalar, 4, 1> projectedk =
+ region.static_k() - region.static_H() * perpendicular_vector * w / norm_K;
Scalar min_boundary = ::std::numeric_limits<Scalar>::lowest();
Scalar max_boundary = ::std::numeric_limits<Scalar>::max();
@@ -60,11 +80,16 @@
vertices << max_boundary, min_boundary;
if (max_boundary > min_boundary) {
+ // The line goes through the region (if the line just intersects a single
+ // vertex, then we fall through to the next clause), but R is not
+ // inside the region. The returned point will be one of the two points where
+ // the line intersects the edge of the region.
Scalar min_distance_sqr = 0;
Eigen::Matrix<Scalar, 2, 1> closest_point;
for (int i = 0; i < vertices.innerSize(); i++) {
Eigen::Matrix<Scalar, 2, 1> point;
- point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
+ point =
+ parallel_vector * vertices(0, i) + perpendicular_vector * w / norm_K;
const Scalar length = (R - point).squaredNorm();
if (i == 0 || length < min_distance_sqr) {
closest_point = point;
@@ -74,6 +99,8 @@
if (is_inside) *is_inside = true;
return closest_point;
} else {
+ // The line does not pass through the region; identify the vertex closest to
+ // the line.
Eigen::Matrix<Scalar, 2, 4> region_vertices = region.StaticVertices();
#ifdef __linux__
CHECK_GT(reinterpret_cast<ssize_t>(region_vertices.outerSize()), 0);
@@ -83,8 +110,11 @@
Scalar min_distance = INFINITY;
int closest_i = 0;
for (int i = 0; i < region_vertices.outerSize(); i++) {
+ // Calculate the distance of the vertex from the line. The closest vertex
+ // will be the one to return.
const Scalar length = ::std::abs(
- (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
+ (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0) -
+ w);
if (i == 0 || length < min_distance) {
closest_i = i;
min_distance = length;
diff --git a/frc971/control_loops/coerce_goal_test.cc b/frc971/control_loops/coerce_goal_test.cc
index 11a0de9..ae04d6c 100644
--- a/frc971/control_loops/coerce_goal_test.cc
+++ b/frc971/control_loops/coerce_goal_test.cc
@@ -2,31 +2,177 @@
#include <unistd.h>
+#include "aos/controls/polytope.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());
+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;
+ box_H << /*[[*/ 1.0, 0.0 /*]*/,
+ /*[*/ -1.0, 0.0 /*]*/,
+ /*[*/ 0.0, 1.0 /*]*/,
+ /*[*/ 0.0, -1.0 /*]]*/;
+ Eigen::Matrix<double, 4, 1> box_k;
+ box_k << /*[[*/ x1_max /*]*/,
+ /*[*/ -x1_min /*]*/,
+ /*[*/ x2_max /*]*/,
+ /*[*/ -x2_min /*]]*/;
+ aos::controls::HPolytope<2> t_poly(box_H, box_k);
+ return aos::controls::HVPolytope<2, 4, 4>(t_poly.H(), t_poly.k(),
+ t_poly.Vertices());
+}
+} // namespace
+
+class CoerceGoalTest : public ::testing::Test {
+ public:
+ void SetUp() override { aos::controls::HPolytope<2>::Init(); }
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+TEST_F(CoerceGoalTest, Inside) {
+ aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << /*[[*/ 1, -1 /*]]*/;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << /*[[*/ 1.5, 1.5 /*]]*/;
+
+ Eigen::Matrix<double, 2, 1> output =
+ frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
+
+ EXPECT_EQ(R(0, 0), output(0, 0));
+ EXPECT_EQ(R(1, 0), output(1, 0));
}
-TEST(CoerceGoalTest, WithinRegion) {
- const auto upoly = poly();
+TEST_F(CoerceGoalTest, LineOutside) {
+ aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+
+ // Make a line equivalent to y = -x, which does not pass through the box and
+ // is nearest the box at (1, 1).
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1, 1;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << /*[[*/ 0.0, 0.0 /*]]*/;
+
+ Eigen::Matrix<double, 2, 1> output =
+ frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
+
+ EXPECT_EQ(1.0, output(0, 0));
+ EXPECT_EQ(1.0, output(1, 0));
+
+ // Test the same line, but on the other side of the box, where the (2, 2)
+ // vertex will be closest.
+ output = frc971::control_loops::CoerceGoal<double>(box, K, 5, R);
+ EXPECT_EQ(2.0, output(0, 0));
+ EXPECT_EQ(2.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, GoalOutsideLineInsideThroughOrigin) {
+ aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1, -1;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << 5, 5;
+
+ Eigen::Matrix<double, 2, 1> output =
+ frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
+
+ EXPECT_EQ(2.0, output(0, 0));
+ EXPECT_EQ(2.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, GoalOutsideLineNotThroughOrigin) {
+ aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1, 1;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << 0, 3;
+
+ Eigen::Matrix<double, 2, 1> output =
+ frc971::control_loops::CoerceGoal<double>(box, K, 3, R);
+
+ EXPECT_EQ(1.0, output(0, 0));
+ EXPECT_DOUBLE_EQ(2.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, GoalOutsideLineThroughVertex) {
+ aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1, -1;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << 5, 5;
+
+ Eigen::Matrix<double, 2, 1> output =
+ frc971::control_loops::CoerceGoal<double>(box, K, 1, R);
+
+ EXPECT_EQ(2.0, output(0, 0));
+ EXPECT_EQ(1.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, LineAndGoalOutside) {
+ aos::controls::HVPolytope<2, 4, 4> box = MakeBox(3, 4, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1, -1;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << 5, 5;
+
+ Eigen::Matrix<double, 2, 1> output =
+ frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
+
+ EXPECT_EQ(3.0, output(0, 0));
+ EXPECT_EQ(2.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, LineThroughEdgeOfBox) {
+ aos::controls::HVPolytope<2, 4, 4> box = MakeBox(0, 4, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << -1, 1;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << 5, 5;
+
+ Eigen::Matrix<double, 2, 1> output =
+ frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
+
+ EXPECT_EQ(2.0, output(0, 0));
+ EXPECT_EQ(2.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, PerpendicularLine) {
+ aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
+
+ Eigen::Matrix<double, 1, 2> K;
+ K << 1, 1;
+
+ Eigen::Matrix<double, 2, 1> R;
+ R << 5, 5;
+
+ Eigen::Matrix<double, 2, 1> output =
+ frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
+
+ EXPECT_EQ(1.0, output(0, 0));
+ EXPECT_EQ(1.0, output(1, 0));
+}
+
+TEST_F(CoerceGoalTest, WithinRegion) {
+ const auto upoly = MakeBox(-12.0, 12.0, -12.0, 12.0);
Eigen::Matrix<double, 1, 2> k;
k << 2, 2;
@@ -35,12 +181,12 @@
auto result = CoerceGoal<double>(upoly, k, 0, goal);
- EXPECT_EQ(result(0, 0), -2);
- EXPECT_EQ(result(1, 0), 2);
+ EXPECT_EQ(result(0, 0), goal(0, 0));
+ EXPECT_EQ(result(1, 0), goal(1, 0));
}
-TEST(CoerceGoalTest, VerticalLine) {
- const auto upoly = poly();
+TEST_F(CoerceGoalTest, VerticalLine) {
+ const auto upoly = MakeBox(-12.0, 12.0, -12.0, 12.0);
Eigen::Matrix<double, 1, 2> k;
k << 2, 0;
@@ -53,8 +199,8 @@
EXPECT_EQ(result(1, 0), 12);
}
-TEST(CoerceGoalTest, HorizontalLine) {
- const auto upoly = poly();
+TEST_F(CoerceGoalTest, HorizontalLine) {
+ const auto upoly = MakeBox(-12.0, 12.0, -12.0, 12.0);
Eigen::Matrix<double, 1, 2> k;
k << 0, 2;
@@ -67,6 +213,5 @@
EXPECT_EQ(result(1, 0), 0);
}
-} // namespace
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 3b668dd..a289fed 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -1414,109 +1414,6 @@
EXPECT_EQ(1.0, localizer_.theta());
}
-::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;
- box_H << /*[[*/ 1.0, 0.0 /*]*/,
- /*[*/ -1.0, 0.0 /*]*/,
- /*[*/ 0.0, 1.0 /*]*/,
- /*[*/ 0.0, -1.0 /*]]*/;
- Eigen::Matrix<double, 4, 1> box_k;
- box_k << /*[[*/ x1_max /*]*/,
- /*[*/ -x1_min /*]*/,
- /*[*/ x2_max /*]*/,
- /*[*/ -x2_min /*]]*/;
- ::aos::controls::HPolytope<2> t_poly(box_H, box_k);
- return ::aos::controls::HVPolytope<2, 4, 4>(t_poly.H(), t_poly.k(),
- t_poly.Vertices());
-}
-
-class CoerceGoalTest : public ::testing::Test {
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
-
-// WHOOOHH!
-TEST_F(CoerceGoalTest, Inside) {
- ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
-
- Eigen::Matrix<double, 1, 2> K;
- K << /*[[*/ 1, -1 /*]]*/;
-
- Eigen::Matrix<double, 2, 1> R;
- R << /*[[*/ 1.5, 1.5 /*]]*/;
-
- Eigen::Matrix<double, 2, 1> output =
- ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
-
- EXPECT_EQ(R(0, 0), output(0, 0));
- EXPECT_EQ(R(1, 0), output(1, 0));
-}
-
-TEST_F(CoerceGoalTest, Outside_Inside_Intersect) {
- ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
-
- Eigen::Matrix<double, 1, 2> K;
- K << 1, -1;
-
- Eigen::Matrix<double, 2, 1> R;
- R << 5, 5;
-
- Eigen::Matrix<double, 2, 1> output =
- ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
-
- EXPECT_EQ(2.0, output(0, 0));
- EXPECT_EQ(2.0, output(1, 0));
-}
-
-TEST_F(CoerceGoalTest, Outside_Inside_no_Intersect) {
- ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(3, 4, 1, 2);
-
- Eigen::Matrix<double, 1, 2> K;
- K << 1, -1;
-
- Eigen::Matrix<double, 2, 1> R;
- R << 5, 5;
-
- Eigen::Matrix<double, 2, 1> output =
- ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
-
- EXPECT_EQ(3.0, output(0, 0));
- EXPECT_EQ(2.0, output(1, 0));
-}
-
-TEST_F(CoerceGoalTest, Middle_Of_Edge) {
- ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(0, 4, 1, 2);
-
- Eigen::Matrix<double, 1, 2> K;
- K << -1, 1;
-
- Eigen::Matrix<double, 2, 1> R;
- R << 5, 5;
-
- Eigen::Matrix<double, 2, 1> output =
- ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
-
- EXPECT_EQ(2.0, output(0, 0));
- EXPECT_EQ(2.0, output(1, 0));
-}
-
-TEST_F(CoerceGoalTest, PerpendicularLine) {
- ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
-
- Eigen::Matrix<double, 1, 2> K;
- K << 1, 1;
-
- Eigen::Matrix<double, 2, 1> R;
- R << 5, 5;
-
- Eigen::Matrix<double, 2, 1> output =
- ::frc971::control_loops::CoerceGoal<double>(box, K, 0, R);
-
- EXPECT_EQ(1.0, output(0, 0));
- EXPECT_EQ(1.0, output(1, 0));
-}
-
// TODO(austin): Make sure the profile reset code when we disable works.
} // namespace testing