tuned and redid the capping on the claw to prioritize separation error
diff --git a/aos/controls/polytope.h b/aos/controls/polytope.h
index a873722..c3f66cc 100644
--- a/aos/controls/polytope.h
+++ b/aos/controls/polytope.h
@@ -42,10 +42,10 @@
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);
+ 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();
+ Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices() const;
private:
Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> H_;
@@ -54,7 +54,7 @@
template <int number_of_dimensions>
bool HPolytope<number_of_dimensions>::IsInside(
- Eigen::Matrix<double, number_of_dimensions, 1> point) {
+ 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)) {
@@ -66,7 +66,7 @@
template <int number_of_dimensions>
Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
- HPolytope<number_of_dimensions>::Vertices() {
+ HPolytope<number_of_dimensions>::Vertices() const {
dd_MatrixPtr matrix = dd_CreateMatrix(num_constraints(), ndim() + 1);
// Copy the data over. TODO(aschuh): Is there a better way? I hate copying...