Finish up the ARM MPC as far as I could get before abandoning
Turns out this is a bit of a dead end due to the solver employed. I'll
have to revisit it in the future.
Change-Id: Ib75a053395afa6f31dee3ba6c20a236c7c0b433f
diff --git a/y2018/control_loops/python/3d_plot.cc b/y2018/control_loops/python/3d_plot.cc
new file mode 100644
index 0000000..ff1e293
--- /dev/null
+++ b/y2018/control_loops/python/3d_plot.cc
@@ -0,0 +1,75 @@
+#include <chrono>
+#include <cmath>
+#include <thread>
+
+#include "third_party/gflags/include/gflags/gflags.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#include "y2018/control_loops/python/arm_bounds.h"
+
+DEFINE_double(boundary_scalar, 20000.0, "quadratic slope");
+DEFINE_double(boundary_rate, 1000.0, "linear slope");
+DEFINE_double(bounds_offset, 0.02, "Offset the quadratic boundary in by this");
+
+using ::y2018::control_loops::Point;
+
+int main(int argc, char **argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
+
+ ::y2018::control_loops::BoundsCheck arm_space =
+ ::y2018::control_loops::MakeClippedArmSpace();
+
+ matplotlibcpp::figure();
+ ::std::vector<double> bounds_x;
+ ::std::vector<double> bounds_y;
+ for (const Point p : arm_space.points()) {
+ bounds_x.push_back(p.x());
+ bounds_y.push_back(p.y());
+ }
+ matplotlibcpp::plot(bounds_x, bounds_y, {{"label", "actual region"}});
+ matplotlibcpp::legend();
+
+ ::std::vector<::std::vector<double>> cost_x;
+ ::std::vector<::std::vector<double>> cost_y;
+ ::std::vector<::std::vector<double>> cost_z;
+ ::std::vector<::std::vector<double>> cost_z2;
+ ::std::vector<::std::vector<double>> cost_z3;
+
+ for (double x_coordinate = -0.5; x_coordinate < 1.2; x_coordinate += 0.05) {
+ ::std::vector<double> cost_x_row;
+ ::std::vector<double> cost_y_row;
+ ::std::vector<double> cost_z_row;
+ ::std::vector<double> cost_z2_row;
+ ::std::vector<double> cost_z3_row;
+
+ for (double y_coordinate = -1.0; y_coordinate < 5.0; y_coordinate += 0.05) {
+ cost_x_row.push_back(x_coordinate);
+ cost_y_row.push_back(y_coordinate);
+
+ ::Eigen::Matrix<double, 2, 1> norm;
+ double min_distance =
+ arm_space.min_distance(Point(x_coordinate, y_coordinate), &norm);
+ cost_z_row.push_back(::std::min(
+ 500.0 * ::std::max(min_distance + FLAGS_bounds_offset, 0.0), 40.0));
+
+ cost_z2_row.push_back(::std::min(
+ FLAGS_boundary_scalar *
+ ::std::max(0.0, min_distance + FLAGS_bounds_offset) *
+ ::std::max(0.0, min_distance + FLAGS_bounds_offset) +
+ FLAGS_boundary_rate *
+ ::std::max(0.0, min_distance + FLAGS_bounds_offset),
+ 200.0));
+
+ cost_z3_row.push_back(min_distance);
+ }
+ cost_x.push_back(cost_x_row);
+ cost_y.push_back(cost_y_row);
+ cost_z.push_back(cost_z_row);
+ cost_z2.push_back(cost_z2_row);
+ cost_z3.push_back(cost_z3_row);
+ }
+
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_z);
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_z2);
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_z3);
+ matplotlibcpp::show();
+}