Improve the C++ libcdd wrapper
* Tests!
* It now pre-calculates vertices, because we always do that at least
once and it makes the next thing easier.
* Support directly transforming vertices in the form the control loops
do to avoid libcdd weirdness with some transformed ones.
* Drop broken error-handling which just resulted in an Eigen assertion
instead of actually printing out the error.
Change-Id: I2b184011d78b7479d5c14409b946a5f0b1c2bf3c
diff --git a/aos/common/controls/BUILD b/aos/common/controls/BUILD
index 5bb31fb..9cc6f31 100644
--- a/aos/common/controls/BUILD
+++ b/aos/common/controls/BUILD
@@ -40,6 +40,22 @@
deps = [
'//third_party/eigen',
'//third_party/cddlib',
+ '//aos/common/logging',
+ '//aos/common/logging:matrix_logging',
+ ],
+)
+
+cc_test(
+ name = 'polytope_test',
+ srcs = [
+ 'polytope_test.cc',
+ ],
+ deps = [
+ ':polytope',
+ '//aos/testing:googletest',
+ '//third_party/eigen',
+ '//third_party/googletest:googlemock',
+ '//aos/testing:test_logging',
],
)
diff --git a/aos/common/controls/polytope.h b/aos/common/controls/polytope.h
index 4efa628..ccd257b 100644
--- a/aos/common/controls/polytope.h
+++ b/aos/common/controls/polytope.h
@@ -5,39 +5,67 @@
#include "third_party/cddlib/lib-src/setoper.h"
#include "third_party/cddlib/lib-src/cdd.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/matrix_logging.h"
+
namespace aos {
namespace controls {
-// A n dimension polytope.
+// A number_of_dimensions dimensional polytope.
+// This represents the region consisting of all points X such that H * X <= k.
+// The vertices are calculated at construction time because we always use those
+// and libcdd is annoying about calculating vertices. In particular, for some
+// random-seeming polytopes it refuses to calculate the vertices completely. To
+// avoid issues with that, using the "shifting" constructor is recommended
+// whenever possible.
template <int number_of_dimensions>
class HPolytope {
public:
- // Constructs a polytope given the H and k matricies.
- HPolytope(Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> H,
- Eigen::Matrix<double, Eigen::Dynamic, 1> k)
- : H_(H),
- k_(k) {
- }
+ // Constructs a polytope given the H and k matrices.
+ // Do NOT use this to calculate a polytope derived from another one in a way
+ // another constructor can be used instead.
+ HPolytope(
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> &H,
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> &k)
+ : H_(H), k_(k), vertices_(CalculateVertices(H, k)) {}
+
+ // Constructs a polytope with H = other.H() * H_multiplier and
+ // k = other.k() + other.H() * k_shifter.
+ // This avoids calculating the vertices for the new H and k directly because
+ // that sometimes confuses libcdd depending on what exactly the new H and k
+ // are.
+ //
+ // This currently has the following restrictions (CHECKed internally) because
+ // we don't have uses for anything else:
+ // If number_of_dimensions is not 1, it must be 2, other.H() *
+ // H_multiplier.inverse() * k_shifter must have its first 2 columns and last
+ // 2 columns as opposites, and the 1st+2nd and 3rd+4th vertices must be
+ // normal to each other.
+ HPolytope(const HPolytope<number_of_dimensions> &other,
+ const Eigen::Matrix<double, number_of_dimensions,
+ number_of_dimensions> &H_multiplier,
+ const Eigen::Matrix<double, number_of_dimensions, 1> &k_shifter)
+ : H_(other.H() * H_multiplier),
+ k_(other.k() + other.H() * k_shifter),
+ vertices_(
+ ShiftVertices(CalculateVertices(H_, other.k()),
+ other.H() * H_multiplier.inverse() * k_shifter)) {}
// This is an initialization function shared across all instantiations of this
// template.
- // This must be called at least once before calling any of the methods. It is
+ // This must be called at least once before creating any instances. It is
// not thread-safe.
static void Init() {
dd_set_global_constants();
}
// Returns a reference to H.
- const Eigen::Matrix<double, Eigen::Dynamic,
- number_of_dimensions> &H() const {
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> &H() const {
return H_;
}
// Returns a reference to k.
- const Eigen::Matrix<double, Eigen::Dynamic,
- 1> &k() const {
- return k_;
- }
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> &k() const { return k_; }
// Returns the number of dimensions in the polytope.
int ndim() const { return number_of_dimensions; }
@@ -49,11 +77,63 @@
bool IsInside(Eigen::Matrix<double, number_of_dimensions, 1> point) const;
// Returns the list of vertices inside the polytope.
- Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices() const;
+ const Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> &Vertices()
+ const {
+ return vertices_;
+ }
private:
- Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> H_;
- Eigen::Matrix<double, Eigen::Dynamic, 1> k_;
+ static Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
+ CalculateVertices(
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> &H,
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> &k);
+
+ static Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
+ ShiftVertices(const Eigen::Matrix<double, number_of_dimensions,
+ Eigen::Dynamic> &vertices,
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> &shift) {
+ static_assert(number_of_dimensions <= 2, "not implemented yet");
+ if (vertices.cols() != number_of_dimensions * 2) {
+ LOG(FATAL,
+ "less vertices not supported yet: %zd vertices vs %d dimensions\n",
+ vertices.cols(), number_of_dimensions);
+ }
+ if (number_of_dimensions == 2) {
+ if ((shift.row(0) + shift.row(1)).norm() > 0.0001) {
+ LOG_MATRIX(FATAL, "bad shift amount", shift.row(0) + shift.row(1));
+ }
+ if ((shift.row(2) + shift.row(3)).norm() > 0.0001) {
+ LOG_MATRIX(FATAL, "bad shift amount", shift.row(2) + shift.row(3));
+ }
+ if (vertices.col(0).dot(vertices.col(1)) > 0.0001) {
+ ::Eigen::Matrix<double, number_of_dimensions, 2> bad;
+ bad.col(0) = vertices.col(0);
+ bad.col(1) = vertices.col(1);
+ LOG_MATRIX(FATAL, "bad vertices", bad);
+ }
+ if (vertices.col(2).dot(vertices.col(3)) > 0.0001) {
+ ::Eigen::Matrix<double, number_of_dimensions, 2> bad;
+ bad.col(0) = vertices.col(2);
+ bad.col(1) = vertices.col(3);
+ LOG_MATRIX(FATAL, "bad vertices", bad);
+ }
+ }
+
+ Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> r(
+ number_of_dimensions, vertices.cols());
+ Eigen::Matrix<double, number_of_dimensions, 1> real_shift;
+ real_shift(0, 0) = shift(0, 0);
+ if (number_of_dimensions == 2) real_shift(1, 0) = shift(2, 0);
+ for (int i = 0; i < vertices.cols(); ++i) {
+ r.col(i) = vertices.col(i) + real_shift;
+ }
+ return r;
+ }
+
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> H_;
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> k_;
+
+ const Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> vertices_;
};
template <int number_of_dimensions>
@@ -70,14 +150,16 @@
template <int number_of_dimensions>
Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
- HPolytope<number_of_dimensions>::Vertices() const {
- dd_MatrixPtr matrix = dd_CreateMatrix(num_constraints(), ndim() + 1);
+HPolytope<number_of_dimensions>::CalculateVertices(
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> &H,
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> &k) {
+ dd_MatrixPtr matrix = dd_CreateMatrix(k.rows(), number_of_dimensions + 1);
// Copy the data over. TODO(aschuh): Is there a better way? I hate copying...
- for (int i = 0; i < num_constraints(); ++i) {
- dd_set_d(matrix->matrix[i][0], k_(i, 0));
- for (int j = 0; j < ndim(); ++j) {
- dd_set_d(matrix->matrix[i][j + 1], -H_(i, j));
+ for (int i = 0; i < k.rows(); ++i) {
+ dd_set_d(matrix->matrix[i][0], k(i, 0));
+ for (int j = 0; j < number_of_dimensions; ++j) {
+ dd_set_d(matrix->matrix[i][j + 1], -H(i, j));
}
}
@@ -89,8 +171,9 @@
if (error != dd_NoError || polyhedra == NULL) {
dd_WriteErrorMessages(stderr, error);
dd_FreeMatrix(matrix);
- Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> ans(0, 0);
- return ans;
+ LOG_MATRIX(ERROR, "bad H", H);
+ LOG_MATRIX(ERROR, "bad k_", k);
+ LOG(FATAL, "dd_DDMatrix2Poly failed\n");
}
dd_MatrixPtr vertex_matrix = dd_CopyGenerators(polyhedra);
@@ -105,6 +188,9 @@
}
}
+ // Rays are unsupported right now. This may change in the future.
+ CHECK_EQ(0, num_rays);
+
Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> vertices(
number_of_dimensions, num_vertices);
diff --git a/aos/common/controls/polytope_test.cc b/aos/common/controls/polytope_test.cc
new file mode 100644
index 0000000..b458b74
--- /dev/null
+++ b/aos/common/controls/polytope_test.cc
@@ -0,0 +1,154 @@
+#include "aos/common/controls/polytope.h"
+
+#include <vector>
+
+#include "Eigen/Dense"
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+
+#include "aos/testing/test_logging.h"
+
+namespace aos {
+namespace controls {
+
+class HPolytopeTest : public ::testing::Test {
+ protected:
+ HPolytope<2> Polytope1() {
+ return HPolytope<2>{
+ (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()};
+ }
+ HPolytope<2> Polytope2() {
+ return HPolytope<2>{
+ (Eigen::Matrix<double, 4, 2>() << 1, 1, -1, -1, 0, 1, 0, -1).finished(),
+ (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0).finished()};
+ }
+ HPolytope<1> Polytope3() {
+ return HPolytope<1>{(Eigen::Matrix<double, 2, 1>() << 1, -0.5).finished(),
+ (Eigen::Matrix<double, 2, 1>() << 5.0, 2.0).finished()};
+ }
+ HPolytope<2> Polytope4() {
+ return HPolytope<2>{
+ (Eigen::Matrix<double, 4, 2>() << 1, 1, -1, -1, 1, -1, -1, 1)
+ .finished(),
+ (Eigen::Matrix<double, 4, 1>() << 2, -1, 2, -1).finished()};
+ }
+ HPolytope<2> Polytope5() {
+ return HPolytope<2>{
+ (Eigen::Matrix<double, 4, 2>() << 1, 1, -1, -1, 1, -1, -1, 1)
+ .finished(),
+ (Eigen::Matrix<double, 4, 1>() << 1.5, -0.5, 1.5, -0.5).finished()};
+ }
+
+ void SetUp() override {
+ ::aos::testing::EnableTestLogging();
+ ::aos::testing::ForcePrintLogsDuringTests();
+ HPolytope<0>::Init();
+ }
+
+ template <typename T>
+ ::std::vector<::std::vector<double>> MatrixToVectors(const T &matrix) {
+ ::std::vector<::std::vector<double>> r;
+ for (int i = 0; i < matrix.cols(); ++i) {
+ ::std::vector<double> col;
+ for (int j = 0; j < matrix.rows(); ++j) {
+ col.emplace_back(matrix(j, i));
+ }
+ r.emplace_back(col);
+ }
+ return r;
+ }
+
+ template <typename T>
+ ::std::vector<::testing::Matcher<::std::vector<double>>> MatrixToMatchers(
+ const T &matrix) {
+ ::std::vector<::testing::Matcher<::std::vector<double>>> r;
+ for (int i = 0; i < matrix.cols(); ++i) {
+ ::std::vector<::testing::Matcher<double>> col;
+ for (int j = 0; j < matrix.rows(); ++j) {
+ col.emplace_back(::testing::DoubleNear(matrix(j, i), 0.000001));
+ }
+ r.emplace_back(::testing::ElementsAreArray(col));
+ }
+ return r;
+ }
+
+ template <int number_of_dimensions>
+ void CheckShiftedVertices(
+ const HPolytope<number_of_dimensions> &base,
+ const Eigen::Matrix<double, number_of_dimensions, number_of_dimensions> &
+ H_multiplier,
+ const Eigen::Matrix<double, number_of_dimensions, 1> &k_shifter) {
+ LOG_MATRIX(DEBUG, "base vertices", base.Vertices());
+ const auto shifted = HPolytope<number_of_dimensions>(
+ base.H() * H_multiplier, base.k() + base.H() * k_shifter);
+ LOG_MATRIX(DEBUG, "shifted vertices", shifted.Vertices());
+ LOG_MATRIX(DEBUG, "shifted - base", shifted.Vertices() - base.Vertices());
+ EXPECT_THAT(MatrixToVectors(HPolytope<number_of_dimensions>(
+ base, H_multiplier, k_shifter).Vertices()),
+ ::testing::UnorderedElementsAreArray(
+ MatrixToMatchers(shifted.Vertices())));
+ }
+};
+
+// Tests that the vertices for various polytopes calculated from H and k are
+// correct.
+TEST_F(HPolytopeTest, CalculatedVertices) {
+ EXPECT_THAT(MatrixToVectors(Polytope1().Vertices()),
+ ::testing::UnorderedElementsAreArray(
+ MatrixToVectors((Eigen::Matrix<double, 2, 4>() << -12, -12,
+ 12, 12, -12, 12, 12, -12).finished())));
+ EXPECT_THAT(MatrixToVectors(Polytope2().Vertices()),
+ ::testing::UnorderedElementsAreArray(
+ MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 24, 0, -24,
+ 0, -12, 12, 12, -12).finished())));
+ EXPECT_THAT(MatrixToVectors(Polytope3().Vertices()),
+ ::testing::UnorderedElementsAreArray(MatrixToVectors(
+ (Eigen::Matrix<double, 1, 2>() << 5, -4).finished())));
+ EXPECT_THAT(MatrixToVectors(Polytope4().Vertices()),
+ ::testing::UnorderedElementsAreArray(
+ MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 1, 1.5, 1.5,
+ 2, 0, -0.5, 0.5, 0).finished())));
+ EXPECT_THAT(MatrixToVectors(Polytope5().Vertices()),
+ ::testing::UnorderedElementsAreArray(
+ MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 0.5, 1, 1.5,
+ 1, 0, 0.5, 0, -0.5).finished())));
+}
+
+TEST_F(HPolytopeTest, ShiftedVertices) {
+ CheckShiftedVertices(
+ Polytope1(), (Eigen::Matrix<double, 2, 2>() << 1, 1, 1, -1).finished(),
+ (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
+ CheckShiftedVertices(
+ Polytope1(), (Eigen::Matrix<double, 2, 2>() << 1, -1, 1, 1).finished(),
+ (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
+ CheckShiftedVertices(
+ Polytope1(), (Eigen::Matrix<double, 2, 2>() << 1, 1, 1.5, -1).finished(),
+ (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
+ CheckShiftedVertices(
+ Polytope1(),
+ (Eigen::Matrix<double, 2, 2>() << 1, 1.5, -1.5, -1).finished(),
+ (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
+ CheckShiftedVertices(
+ Polytope1(),
+ (Eigen::Matrix<double, 2, 2>() << 1, -1.5, -1.5, -1).finished(),
+ (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
+ CheckShiftedVertices(
+ Polytope1(),
+ (Eigen::Matrix<double, 2, 2>() << 2, -1.5, -1.5, -1).finished(),
+ (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
+ CheckShiftedVertices(
+ Polytope1(),
+ (Eigen::Matrix<double, 2, 2>() << 2, -1.5, -1.25, -1).finished(),
+ (Eigen::Matrix<double, 2, 1>() << 9.71, 2.54).finished());
+
+ CheckShiftedVertices(Polytope3(),
+ (Eigen::Matrix<double, 1, 1>() << 1).finished(),
+ (Eigen::Matrix<double, 1, 1>() << 9.71).finished());
+ CheckShiftedVertices(Polytope3(),
+ (Eigen::Matrix<double, 1, 1>() << -2.54).finished(),
+ (Eigen::Matrix<double, 1, 1>() << 16.78).finished());
+}
+
+} // namespace controls
+} // namespace aos