Removed mallocs from polydrive code
Since most of the places where we create a polytope are just affine
transformations of another polytope, we can transform the original
vertices to avoid needing libcdd at runtime. This is in preparation
for the battlebot.
Change-Id: Ic35af2d918095dc23b90a4ab248d0c142cf3588a
diff --git a/aos/common/controls/polytope.h b/aos/common/controls/polytope.h
index ccd257b..6973867 100644
--- a/aos/common/controls/polytope.h
+++ b/aos/common/controls/polytope.h
@@ -19,129 +19,140 @@
// avoid issues with that, using the "shifting" constructor is recommended
// whenever possible.
template <int number_of_dimensions>
-class HPolytope {
+class Polytope {
public:
- // 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 creating any instances. It is
- // not thread-safe.
- static void Init() {
- dd_set_global_constants();
- }
+ virtual ~Polytope() {}
// Returns a reference to H.
- const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> &H() const {
- return H_;
- }
+ virtual Eigen::Ref<
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>>
+ H() const = 0;
// Returns a reference to k.
- const Eigen::Matrix<double, Eigen::Dynamic, 1> &k() const { return k_; }
+ virtual Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k()
+ const = 0;
// Returns the number of dimensions in the polytope.
- int ndim() const { return number_of_dimensions; }
+ constexpr int ndim() const { return number_of_dimensions; }
// Returns the number of constraints currently in the polytope.
- int num_constraints() const { return k_.rows(); }
+ int num_constraints() const { return k().rows(); }
// Returns true if the point is inside the polytope.
bool IsInside(Eigen::Matrix<double, number_of_dimensions, 1> point) const;
// Returns the list of vertices inside the polytope.
- const Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> &Vertices()
+ virtual Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
+ const = 0;
+};
+
+template <int number_of_dimensions, int num_vertices>
+Eigen::Matrix<double, number_of_dimensions, num_vertices> ShiftPoints(
+ const Eigen::Matrix<double, number_of_dimensions, num_vertices> &vertices,
+ const Eigen::Matrix<double, number_of_dimensions, 1> &offset) {
+ Eigen::Matrix<double, number_of_dimensions, num_vertices> ans = vertices;
+ for (int i = 0; i < num_vertices; ++i) {
+ ans.col(i) += offset;
+ }
+ return ans;
+}
+
+template <int number_of_dimensions, int num_constraints, int num_vertices>
+class HVPolytope : public Polytope<number_of_dimensions> {
+ public:
+ // Constructs a polytope given the H and k matrices.
+ HVPolytope(Eigen::Ref<const Eigen::Matrix<double, num_constraints,
+ number_of_dimensions>> H,
+ Eigen::Ref<const Eigen::Matrix<double, num_constraints, 1>> k,
+ Eigen::Ref<const Eigen::Matrix<double, number_of_dimensions,
+ num_vertices>> vertices)
+ : H_(H), k_(k), vertices_(vertices) {}
+
+ Eigen::Ref<const Eigen::Matrix<double, num_constraints, number_of_dimensions>>
+ static_H() const {
+ return H_;
+ }
+
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>>
+ H() const override {
+ return H_;
+ }
+
+ Eigen::Ref<const Eigen::Matrix<double, num_constraints, 1>> static_k()
const {
+ return k_;
+ }
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k()
+ const override {
+ return k_;
+ }
+
+ Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
+ const override {
+ return vertices_;
+ }
+
+ Eigen::Matrix<double, number_of_dimensions, num_vertices>
+ StaticVertices() const {
return vertices_;
}
private:
- 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);
+ const Eigen::Matrix<double, num_constraints, number_of_dimensions> H_;
+ const Eigen::Matrix<double, num_constraints, 1> k_;
+ const Eigen::Matrix<double, number_of_dimensions, num_vertices> vertices_;
+};
- 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;
+
+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)
+ : H_(H), k_(k), vertices_(CalculateVertices(H, k)) {}
+
+ // This is an initialization function shared across all instantiations of this
+ // template.
+ // This must be called at least once before creating any instances. It is
+ // not thread-safe.
+ static void Init() { dd_set_global_constants(); }
+
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>>
+ H() const override {
+ return H_;
+ }
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k()
+ const override {
+ return k_;
}
+ Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
+ const override {
+ return vertices_;
+ }
+
+ private:
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_;
+
+ static Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
+ CalculateVertices(
+ Eigen::Ref<
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> &H,
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> &k);
};
template <int number_of_dimensions>
-bool HPolytope<number_of_dimensions>::IsInside(
+bool Polytope<number_of_dimensions>::IsInside(
Eigen::Matrix<double, number_of_dimensions, 1> point) const {
- auto ev = H_ * point;
- for (int i = 0; i < num_constraints(); ++i) {
- if (ev(i, 0) > k_(i, 0)) {
+ auto k_ptr = k();
+ auto ev = H() * point;
+ for (int i = 0; i < k_ptr.rows(); ++i) {
+ if (ev(i, 0) > k_ptr(i, 0)) {
return false;
}
}
@@ -151,8 +162,9 @@
template <int number_of_dimensions>
Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
HPolytope<number_of_dimensions>::CalculateVertices(
- const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> &H,
- const Eigen::Matrix<double, Eigen::Dynamic, 1> &k) {
+ Eigen::Ref<
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> &H,
+ Eigen::Ref<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...
diff --git a/aos/common/controls/polytope_test.cc b/aos/common/controls/polytope_test.cc
index b458b74..a43b056 100644
--- a/aos/common/controls/polytope_test.cc
+++ b/aos/common/controls/polytope_test.cc
@@ -72,23 +72,6 @@
}
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
@@ -115,40 +98,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
diff --git a/frc971/control_loops/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
index 0868c2c..0c98560 100644
--- a/frc971/control_loops/coerce_goal.cc
+++ b/frc971/control_loops/coerce_goal.cc
@@ -8,7 +8,7 @@
namespace control_loops {
Eigen::Matrix<double, 2, 1> DoCoerceGoal(
- const aos::controls::HPolytope<2> ®ion,
+ const aos::controls::HVPolytope<2, 4, 4> ®ion,
const Eigen::Matrix<double, 1, 2> &K, double w,
const Eigen::Matrix<double, 2, 1> &R, bool *is_inside) {
if (region.IsInside(R)) {
@@ -20,12 +20,26 @@
perpendicular_vector = K.transpose().normalized();
parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
- aos::controls::HPolytope<1> t_poly(
- region.H() * parallel_vector,
- region.k() - region.H() * perpendicular_vector * w);
+ Eigen::Matrix<double, 4, 1> projectedh = region.static_H() * parallel_vector;
+ Eigen::Matrix<double, 4, 1> projectedk =
+ region.static_k() - region.static_H() * perpendicular_vector * w;
- Eigen::Matrix<double, 1, Eigen::Dynamic> vertices = t_poly.Vertices();
- if (vertices.innerSize() > 0) {
+ double min_boundary = ::std::numeric_limits<double>::lowest();
+ double max_boundary = ::std::numeric_limits<double>::max();
+ for (int i = 0; i < 4; ++i) {
+ if (projectedh(i, 0) > 0) {
+ max_boundary =
+ ::std::min(max_boundary, projectedk(i, 0) / projectedh(i, 0));
+ } else {
+ min_boundary =
+ ::std::max(min_boundary, projectedk(i, 0) / projectedh(i, 0));
+ }
+ }
+
+ Eigen::Matrix<double, 1, 2> vertices;
+ vertices << max_boundary, min_boundary;
+
+ if (max_boundary > min_boundary) {
double min_distance_sqr = 0;
Eigen::Matrix<double, 2, 1> closest_point;
for (int i = 0; i < vertices.innerSize(); i++) {
@@ -40,8 +54,8 @@
if (is_inside) *is_inside = true;
return closest_point;
} else {
- Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
- region.Vertices();
+ Eigen::Matrix<double, 2, 4> region_vertices =
+ region.StaticVertices();
CHECK_GT(region_vertices.outerSize(), 0);
double min_distance = INFINITY;
int closest_i = 0;
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 3933050..83c401d 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -8,21 +8,19 @@
namespace frc971 {
namespace control_loops {
-Eigen::Matrix<double, 2, 1> DoCoerceGoal(const aos::controls::HPolytope<2> ®ion,
- const Eigen::Matrix<double, 1, 2> &K,
- double w,
- const Eigen::Matrix<double, 2, 1> &R,
- bool *is_inside);
+Eigen::Matrix<double, 2, 1> DoCoerceGoal(
+ const aos::controls::HVPolytope<2, 4, 4> ®ion,
+ const Eigen::Matrix<double, 1, 2> &K, double w,
+ const Eigen::Matrix<double, 2, 1> &R, bool *is_inside);
// Intersects a line with a region, and finds the closest point to R.
// Finds a point that is closest to R inside the region, and on the line
// defined by K X = w. If it is not possible to find a point on the line,
// finds a point that is inside the region and closest to the line.
-static inline Eigen::Matrix<double, 2, 1>
- CoerceGoal(const aos::controls::HPolytope<2> ®ion,
- const Eigen::Matrix<double, 1, 2> &K,
- double w,
- const Eigen::Matrix<double, 2, 1> &R) {
+static inline Eigen::Matrix<double, 2, 1> CoerceGoal(
+ const aos::controls::HVPolytope<2, 4, 4> ®ion,
+ const Eigen::Matrix<double, 1, 2> &K, double w,
+ const Eigen::Matrix<double, 2, 1> &R) {
return DoCoerceGoal(region, K, w, R, nullptr);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index aa18aec..8d071dd 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -522,8 +522,8 @@
VerifyNearGoal();
}
-::aos::controls::HPolytope<2> MakeBox(double x1_min, double x1_max,
- double x2_min, double x2_max) {
+::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 /*]*/,
@@ -535,7 +535,8 @@
/*[*/ x2_max /*]*/,
/*[*/ -x2_min /*]]*/;
::aos::controls::HPolytope<2> t_poly(box_H, box_k);
- return t_poly;
+ return ::aos::controls::HVPolytope<2, 4, 4>(t_poly.H(), t_poly.k(),
+ t_poly.Vertices());
}
class CoerceGoalTest : public ::testing::Test {
@@ -545,7 +546,7 @@
// WHOOOHH!
TEST_F(CoerceGoalTest, Inside) {
- ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+ ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
Eigen::Matrix<double, 1, 2> K;
K << /*[[*/ 1, -1 /*]]*/;
@@ -561,7 +562,7 @@
}
TEST_F(CoerceGoalTest, Outside_Inside_Intersect) {
- ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+ ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
Eigen::Matrix<double, 1, 2> K;
K << 1, -1;
@@ -577,7 +578,7 @@
}
TEST_F(CoerceGoalTest, Outside_Inside_no_Intersect) {
- ::aos::controls::HPolytope<2> box = MakeBox(3, 4, 1, 2);
+ ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(3, 4, 1, 2);
Eigen::Matrix<double, 1, 2> K;
K << 1, -1;
@@ -593,7 +594,7 @@
}
TEST_F(CoerceGoalTest, Middle_Of_Edge) {
- ::aos::controls::HPolytope<2> box = MakeBox(0, 4, 1, 2);
+ ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(0, 4, 1, 2);
Eigen::Matrix<double, 1, 2> K;
K << -1, 1;
@@ -609,7 +610,7 @@
}
TEST_F(CoerceGoalTest, PerpendicularLine) {
- ::aos::controls::HPolytope<2> box = MakeBox(1, 2, 1, 2);
+ ::aos::controls::HVPolytope<2, 4, 4> box = MakeBox(1, 2, 1, 2);
Eigen::Matrix<double, 1, 2> K;
K << 1, 1;
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.cc b/frc971/control_loops/drivetrain/polydrivetrain.cc
index 121a231..6d3de59 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.cc
+++ b/frc971/control_loops/drivetrain/polydrivetrain.cc
@@ -16,10 +16,6 @@
namespace control_loops {
namespace drivetrain {
-using ::frc971::control_loops::GearLogging;
-using ::frc971::control_loops::CIMLogging;
-using ::frc971::control_loops::CoerceGoal;
-
PolyDrivetrain::PolyDrivetrain(const DrivetrainConfig &dt_config,
StateFeedbackLoop<7, 2, 3> *kf)
: kf_(kf),
@@ -30,7 +26,9 @@
(Eigen::Matrix<double, 4, 1>() << /*[[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
/*[*/ 12 /*]*/,
- /*[*/ 12 /*]]*/).finished()),
+ /*[*/ 12 /*]]*/).finished(),
+ (Eigen::Matrix<double, 2, 4>() << /*[[*/ 12, 12, -12, -12 /*]*/,
+ /*[*/ -12, 12, 12, -12 /*]*/).finished()),
loop_(new StateFeedbackLoop<2, 2, 2>(dt_config.make_v_drivetrain_loop())),
ttrust_(1.1),
wheel_(0.0),
@@ -239,13 +237,16 @@
const double equality_w = 0.0;
// Construct a constraint on R by manipulating the constraint on U
- ::aos::controls::HPolytope<2> R_poly = ::aos::controls::HPolytope<2>(
- U_Poly_.H() * (loop_->K() + FF),
- U_Poly_.k() + U_Poly_.H() * loop_->K() * loop_->X_hat());
+ ::aos::controls::HVPolytope<2, 4, 4> R_poly_hv(
+ U_Poly_.static_H() * (loop_->K() + FF),
+ U_Poly_.static_k() + U_Poly_.static_H() * loop_->K() * loop_->X_hat(),
+ (loop_->K() + FF).inverse() *
+ ::aos::controls::ShiftPoints<2, 4>(U_Poly_.StaticVertices(),
+ loop_->K() * loop_->X_hat()));
// Limit R back inside the box.
loop_->mutable_R() =
- CoerceGoal(R_poly, equality_k, equality_w, loop_->R());
+ CoerceGoal(R_poly_hv, equality_k, equality_w, loop_->R());
}
const Eigen::Matrix<double, 2, 1> FF_volts = FF * loop_->R();
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 1ea38a7..20f1a49 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -47,7 +47,7 @@
private:
StateFeedbackLoop<7, 2, 3> *kf_;
- const ::aos::controls::HPolytope<2> U_Poly_;
+ const ::aos::controls::HVPolytope<2, 4, 4> U_Poly_;
::std::unique_ptr<StateFeedbackLoop<2, 2, 2>> loop_;
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 235738d..ff7ed6e 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -13,8 +13,6 @@
namespace control_loops {
namespace drivetrain {
-using ::frc971::control_loops::DoCoerceGoal;
-
void DrivetrainMotorsSS::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
output_was_capped_ =
::std::abs((*U)(0, 0)) > 12.0 || ::std::abs((*U)(1, 0)) > 12.0;
@@ -53,9 +51,15 @@
Eigen::Matrix<double, 2, 1> U_integral;
U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
- const ::aos::controls::HPolytope<2> pos_poly(
- U_poly_, position_K * T_,
- -velocity_K * velocity_error + U_integral - kf_->ff_U());
+ const ::aos::controls::HVPolytope<2, 4, 4> pos_poly_hv(
+ U_poly_.static_H() * position_K * T_,
+ U_poly_.static_H() *
+ (-velocity_K * velocity_error + U_integral - kf_->ff_U()) +
+ U_poly_.static_k(),
+ (position_K * T_).inverse() *
+ ::aos::controls::ShiftPoints<2, 4>(
+ U_poly_.StaticVertices(),
+ -velocity_K * velocity_error + U_integral - kf_->ff_U()));
Eigen::Matrix<double, 2, 1> adjusted_pos_error;
{
@@ -81,10 +85,10 @@
bool is_inside_h, is_inside_45;
const auto adjusted_pos_error_h =
- DoCoerceGoal(pos_poly, LH, wh, drive_error, &is_inside_h);
+ DoCoerceGoal(pos_poly_hv, 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)) {
+ DoCoerceGoal(pos_poly_hv, L45, w45, intersection, &is_inside_45);
+ if (pos_poly_hv.IsInside(intersection)) {
adjusted_pos_error = adjusted_pos_error_h;
} else {
if (is_inside_h) {
@@ -115,10 +119,16 @@
double *integrated_kf_heading)
: dt_config_(dt_config),
kf_(kf),
- 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()),
+ U_poly_((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()),
linear_profile_(::aos::controls::kLoopFrequency),
angular_profile_(::aos::controls::kLoopFrequency),
integrated_kf_heading_(integrated_kf_heading) {
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.h b/frc971/control_loops/drivetrain/ssdrivetrain.h
index 15a4823..c066c1b 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.h
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.h
@@ -63,7 +63,7 @@
// 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_;
+ const ::aos::controls::HVPolytope<2, 4, 4> U_poly_;
// multiplying by T converts [left_error, right_error] to
// [left_right_error_difference, total_distance_error].