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/WORKSPACE b/WORKSPACE
index 725df52..5bd89d2 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -250,3 +250,10 @@
strip_prefix = "gcc-arm-none-eabi-7-2018-q2-update/",
url = "http://frc971.org/Build-Dependencies/gcc-arm-none-eabi-7-2018-q2-update-linux.tar.bz2",
)
+
+new_http_archive(
+ name = "cgal_repo",
+ sha256 = "d564dda558570344b4caa66c5bae2cdae9ef68e07829d64f5651b25f2c6a0e9e",
+ url = "http://frc971.org/Build-Dependencies/cgal-dev-4.5-2.tar.gz",
+ build_file = 'debian/cgal.BUILD',
+)
diff --git a/debian/cgal.BUILD b/debian/cgal.BUILD
new file mode 100644
index 0000000..0063d73
--- /dev/null
+++ b/debian/cgal.BUILD
@@ -0,0 +1,18 @@
+
+
+cc_library(
+ name = 'cgal',
+ hdrs = glob([
+ 'usr/include/**/*.h',
+ 'usr/include/**/*.hpp',
+ ]),
+ includes = [
+ 'usr/include',
+ 'usr/include/x86_64-linux-gnu',
+ ],
+ srcs = [
+ 'usr/lib/x86_64-linux-gnu/libgmp.so.10.2.0',
+ 'usr/lib/libCGAL.so.10',
+ ],
+ visibility = ['//visibility:public'],
+)
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h b/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
index 54742eb..86be90e 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
+++ b/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
@@ -198,7 +198,22 @@
solver_.changeInitialState(x_start);
- bool solveSuccessful = solver_.finishMPCIteration();
+ bool solveSuccessful = true;
+
+ bool foundBetter = true;
+ int numIterations = 0;
+
+ while (true) {
+ foundBetter = solver_.finishIteration();
+
+ numIterations++;
+ if (foundBetter &&
+ (numIterations < solver_.getSettings().max_iterations)) {
+ solver_.prepareMPCIteration();
+ } else {
+ break;
+ }
+ }
if (solveSuccessful)
{
diff --git a/third_party/matplotlib-cpp/matplotlibcpp.h b/third_party/matplotlib-cpp/matplotlibcpp.h
index 0391b3f..eb7b7cc 100644
--- a/third_party/matplotlib-cpp/matplotlibcpp.h
+++ b/third_party/matplotlib-cpp/matplotlibcpp.h
@@ -56,6 +56,7 @@
PyObject *s_python_function_errorbar;
PyObject *s_python_function_annotate;
PyObject *s_python_function_tight_layout;
+ PyObject *s_python_colormap;
PyObject *s_python_empty_tuple;
PyObject *s_python_function_stem;
PyObject *s_python_function_xkcd;
@@ -107,9 +108,13 @@
PyObject* matplotlibname = PyString_FromString("matplotlib");
PyObject* pyplotname = PyString_FromString("matplotlib.pyplot");
+ PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits");
+ PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d");
PyObject* pylabname = PyString_FromString("pylab");
- if (!pyplotname || !pylabname || !matplotlibname) {
- throw std::runtime_error("couldnt create string");
+ PyObject* cmname = PyString_FromString("matplotlib.cm");
+ if (!pyplotname || !pylabname || !matplotlibname || !mpl_toolkits ||
+ !axis3d || !cmname) {
+ throw std::runtime_error("couldnt create string");
}
PyObject* matplotlib = PyImport_Import(matplotlibname);
@@ -126,11 +131,22 @@
Py_DECREF(pyplotname);
if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); }
+ s_python_colormap = PyImport_Import(cmname);
+ Py_DECREF(cmname);
+ if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); }
PyObject* pylabmod = PyImport_Import(pylabname);
Py_DECREF(pylabname);
if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); }
+ PyObject* mpl_toolkitsmod = PyImport_Import(mpl_toolkits);
+ Py_DECREF(mpl_toolkitsmod);
+ if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); }
+
+ PyObject* axis3dmod = PyImport_Import(axis3d);
+ Py_DECREF(axis3dmod);
+ if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); }
+
s_python_function_show = PyObject_GetAttrString(pymod, "show");
s_python_function_close = PyObject_GetAttrString(pymod, "close");
s_python_function_draw = PyObject_GetAttrString(pymod, "draw");
@@ -293,6 +309,30 @@
return varray;
}
+template<typename Numeric>
+PyObject* get_2darray(const std::vector<::std::vector<Numeric>>& v)
+{
+ detail::_interpreter::get(); //interpreter needs to be initialized for the numpy commands to work
+ if (v.size() < 1) throw std::runtime_error("get_2d_array v too small");
+
+ npy_intp vsize[2] = {static_cast<npy_intp>(v.size()),
+ static_cast<npy_intp>(v[0].size())};
+
+ PyArrayObject *varray =
+ (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE);
+
+ double *vd_begin = static_cast<double *>(PyArray_DATA(varray));
+
+ for (const ::std::vector<Numeric> &v_row : v) {
+ if (v_row.size() != static_cast<size_t>(vsize[1]))
+ throw std::runtime_error("Missmatched array size");
+ std::copy(v_row.begin(), v_row.end(), vd_begin);
+ vd_begin += vsize[1];
+ }
+
+ return reinterpret_cast<PyObject *>(varray);
+}
+
#else // fallback if we don't have numpy: copy every element of the given vector
template<typename Numeric>
@@ -337,6 +377,76 @@
return res;
}
+template <typename Numeric>
+void plot_surface(const std::vector<::std::vector<Numeric>> &x,
+ const std::vector<::std::vector<Numeric>> &y,
+ const std::vector<::std::vector<Numeric>> &z,
+ const std::map<std::string, std::string> &keywords =
+ std::map<std::string, std::string>()) {
+ assert(x.size() == y.size());
+ assert(y.size() == z.size());
+
+ // using numpy arrays
+ PyObject *xarray = get_2darray(x);
+ PyObject *yarray = get_2darray(y);
+ PyObject *zarray = get_2darray(z);
+
+ // construct positional args
+ PyObject *args = PyTuple_New(3);
+ PyTuple_SetItem(args, 0, xarray);
+ PyTuple_SetItem(args, 1, yarray);
+ PyTuple_SetItem(args, 2, zarray);
+
+ // Build up the kw args.
+ PyObject *kwargs = PyDict_New();
+ PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1));
+ PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1));
+
+ PyObject *python_colormap_coolwarm = PyObject_GetAttrString(
+ detail::_interpreter::get().s_python_colormap, "coolwarm");
+
+ PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm);
+
+ for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
+ it != keywords.end(); ++it) {
+ PyDict_SetItemString(kwargs, it->first.c_str(),
+ PyString_FromString(it->second.c_str()));
+ }
+
+
+ PyObject *fig =
+ PyObject_CallObject(detail::_interpreter::get().s_python_function_figure,
+ detail::_interpreter::get().s_python_empty_tuple);
+ if (!fig) throw std::runtime_error("Call to figure() failed.");
+
+ PyObject *gca_kwargs = PyDict_New();
+ PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d"));
+
+ PyObject *gca = PyObject_GetAttrString(fig, "gca");
+ if (!gca) throw std::runtime_error("No gca");
+ Py_INCREF(gca);
+ PyObject *axis = PyObject_Call(
+ gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs);
+
+ if (!axis) throw std::runtime_error("No axis");
+ Py_INCREF(axis);
+
+ Py_DECREF(gca);
+ Py_DECREF(gca_kwargs);
+
+ PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface");
+ if (!plot_surface) throw std::runtime_error("No surface");
+ Py_INCREF(plot_surface);
+ PyObject *res = PyObject_Call(plot_surface, args, kwargs);
+ if (!res) throw std::runtime_error("failed surface");
+ Py_DECREF(plot_surface);
+
+ Py_DECREF(axis);
+ Py_DECREF(args);
+ Py_DECREF(kwargs);
+ if (res) Py_DECREF(res);
+}
+
template<typename Numeric>
bool stem(const std::vector<Numeric> &x, const std::vector<Numeric> &y, const std::map<std::string, std::string>& keywords)
{
diff --git a/y2018/control_loops/python/2d_plot.cc b/y2018/control_loops/python/2d_plot.cc
new file mode 100644
index 0000000..52b8b34
--- /dev/null
+++ b/y2018/control_loops/python/2d_plot.cc
@@ -0,0 +1,36 @@
+#include <chrono>
+#include <cmath>
+#include <thread>
+
+#include "third_party/gflags/include/gflags/gflags.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+
+DEFINE_double(yrange, 1.0, "+- y max");
+
+double fx(double x, double yrange) {
+ return 2.0 * ((1.0 / (1.0 + ::std::exp(-x * 2.0 / yrange)) - 0.5)) *
+ yrange;
+}
+
+int main(int argc, char **argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
+
+ matplotlibcpp::figure();
+ ::std::vector<double> x;
+ ::std::vector<double> y;
+ ::std::vector<double> slope_y;
+
+ for (double i = -5.0; i < 5.0; i += 0.01) {
+ x.push_back(i);
+ y.push_back(fx(i, FLAGS_yrange));
+ slope_y.push_back(
+ (fx(i + 0.0001, FLAGS_yrange) - fx(i - 0.0001, FLAGS_yrange)) /
+ (2.0 * 0.0001));
+ }
+
+ matplotlibcpp::plot(x, y, {{"label", "saturated x"}});
+ matplotlibcpp::plot(x, slope_y, {{"label", "slope"}});
+ matplotlibcpp::legend();
+ matplotlibcpp::show();
+
+}
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();
+}
diff --git a/y2018/control_loops/python/BUILD b/y2018/control_loops/python/BUILD
index eebbfe2..369dc96 100644
--- a/y2018/control_loops/python/BUILD
+++ b/y2018/control_loops/python/BUILD
@@ -95,14 +95,66 @@
],
)
+cc_library(
+ name = 'dlqr',
+ hdrs = [
+ 'dlqr.h',
+ ],
+ deps = [
+ '@slycot_repo//:slicot',
+ ],
+)
+
cc_binary(
name = "arm_mpc",
srcs = [
"arm_mpc.cc",
],
+ deps = [
+ ":arm_bounds",
+ ':dlqr',
+ "//third_party/ct",
+ "//third_party/gflags",
+ "//third_party/matplotlib-cpp",
+ "@cgal_repo//:cgal",
+ ],
+ restricted_to = ["//tools:k8"],
+)
+
+cc_binary(
+ name = "3d_plot",
+ srcs = [
+ "3d_plot.cc",
+ ],
restricted_to = ["//tools:k8"],
deps = [
- "//third_party/ct",
+ ":arm_bounds",
+ "//third_party/gflags",
+ "//third_party/matplotlib-cpp",
+ ],
+)
+
+cc_library(
+ name = "arm_bounds",
+ srcs = [
+ "arm_bounds.cc",
+ ],
+ hdrs = [
+ "arm_bounds.h",
+ ],
+ deps = [
+ "//third_party/eigen",
+ "@cgal_repo//:cgal",
+ ],
+)
+
+cc_binary(
+ name = "2d_plot",
+ srcs = [
+ "2d_plot.cc",
+ ],
+ restricted_to = ["//tools:k8"],
+ deps = [
"//third_party/gflags",
"//third_party/matplotlib-cpp",
],
diff --git a/y2018/control_loops/python/arm_bounds.cc b/y2018/control_loops/python/arm_bounds.cc
new file mode 100644
index 0000000..2c7b94c
--- /dev/null
+++ b/y2018/control_loops/python/arm_bounds.cc
@@ -0,0 +1,537 @@
+#include "y2018/control_loops/python/arm_bounds.h"
+
+#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
+#include <CGAL/Polygon_2_algorithms.h>
+#include <CGAL/squared_distance_2.h>
+#include <math.h>
+#include <iostream>
+
+namespace y2018 {
+namespace control_loops {
+
+static BoundsCheck MakeArbitraryArmSpace(::std::vector<Point> points) {
+ for (Point &point : points) {
+ point = Point(M_PI / 2.0 - point.x(), M_PI / 2.0 - point.y());
+ }
+ return BoundsCheck(points);
+}
+
+Vector GetNormal(Point pt, Segment segment) {
+ auto perp_line = segment.supporting_line().perpendicular(pt);
+ if (CGAL::do_intersect(perp_line, segment)) {
+ return Vector(segment.supporting_line().projection(pt), pt);
+ } else {
+ if (CGAL::squared_distance(segment.vertex(0), pt) <
+ CGAL::squared_distance(segment.vertex(1), pt)) {
+ return Vector(segment.vertex(0), pt);
+ } else {
+ return Vector(segment.vertex(1), pt);
+ }
+ }
+}
+
+double BoundsCheck::min_distance(Point pt,
+ ::Eigen::Matrix<double, 2, 1> *norm) const {
+ auto *cell = grid_.GetCell(pt);
+ const auto &points = grid_.points();
+ bool inside = false;
+ double dist = 0;
+ Segment best_segment;
+ if (cell) {
+ if (cell->IsBorderline()) {
+ inside = check_inside(pt, points);
+ } else {
+ inside = cell->IsInside(pt);
+ }
+ dist = cell->Distance(pt, &best_segment);
+ } else {
+ dist = min_dist(pt, points, &best_segment);
+ // TODO(austin): Return norm.
+ }
+ Vector normal = GetNormal(pt, best_segment);
+ norm->x() = normal.x();
+ norm->y() = normal.y();
+
+ norm->normalize();
+
+ if (!inside) {
+ *norm = -(*norm);
+ }
+
+ return inside ? -dist : dist;
+}
+
+BoundsCheck MakeFullArmSpace() {
+ return MakeArbitraryArmSpace({{1.8577014383575772, -1.7353804562372057},
+ {1.8322288438826315, -1.761574570216062},
+ {1.7840339881582052, -1.8122752826851365},
+ {1.7367354460218392, -1.863184376466228},
+ {1.69054633425053, -1.9140647421906793},
+ {1.6456590387358871, -1.9646939485526782},
+ {1.6022412524081968, -2.0148691793459164},
+ {1.5604334435784202, -2.0644108218872432},
+ {1.5203477477575325, -2.1131646494909866},
+ {1.4820681545944325, -2.1610026706370666},
+ {1.4456517763321752, -2.207822815007094},
+ {1.4111309389855604, -2.253547685496041},
+ {1.3785158286698027, -2.298122627340264},
+ {1.3477974448369014, -2.3415133576238922},
+ {1.318950649522341, -2.3837033700108243},
+ {1.2919371475531436, -2.4246912899477153},
+ {1.266708279449626, -2.464488312575651},
+ {1.2432075513427807, -2.5031158147261996},
+ {1.2213728618107609, -2.5406031969824228},
+ {1.2011384131259828, -2.5769859833310096},
+ {1.1824363142639513, -2.61230418457078},
+ {1.165197896140849, -2.64660091671642},
+ {1.149354767212547, -2.6799212560849437},
+ {1.134839641107821, -2.712311307404836},
+ {1.121586968578607, -2.7438174590386177},
+ {1.1095334047223224, -2.774485799309971},
+ {1.09861813993779, -2.8043616692157007},
+ {1.088783119985477, -2.8334893289039758},
+ {1.079973177230731, -2.861911717797479},
+ {1.0721360919151535, -2.8896702908486835},
+ {1.065222599283597, -2.916804915950962},
+ {1.0591863556764607, -2.943353819884641},
+ {1.0539838743128325, -2.969353572295195},
+ {1.049574439440948, -2.9948390990606946},
+ {1.0459200058000553, -3.01984371800932},
+ {1.0429850888932353, -3.044399191310831},
+ {1.0407366503803421, -3.0685357900111128},
+ {1.039143981929867, -3.092282367132034},
+ {1.0381785900853944, -3.1156664365461078},
+ {1.0378140840766794, -3.1387142554815886},
+ {1.038026068010855, -3.1614509090414518},
+ {1.0387920384931424, -3.1839003955494407},
+ {1.0400912884293725, -3.2060857118856374},
+ {1.0419048175385754, -3.2280289382581637},
+ {1.0442152499395827, -3.2497513220895704},
+ {1.047006759060362, -3.2712733608874625},
+ {1.050265000044113, -3.2926148841283163},
+ {1.0539770497854413, -3.3137951343195065},
+ {1.0581313547182551, -3.3348328475243303},
+ {1.0627176864909802, -3.355746333744654},
+ {1.0677271057021644, -3.376553557661551},
+ {1.0731519339297724, -3.397272220341514},
+ {1.0789857343709142, -3.4179198426302158},
+ {1.08522330151719, -3.438513851083329},
+ {1.0918606604276442, -3.4590716674313335},
+ {1.0988950763314502, -3.4796108027504618},
+ {1.1063250755031675, -3.5001489577245133},
+ {1.114150478614587, -3.5207041306442908},
+ {1.1223724480923383, -3.541294735118319},
+ {1.1309935514178655, -3.561939729880535},
+ {1.1400178428208325, -3.5826587636046994},
+ {1.1494509664724388, -3.6034723383075913},
+ {1.1593002851278627, -3.6244019957930833},
+ {1.1695750392617108, -3.6454705327253025},
+ {1.1802865431772855, -3.6667022514168814},
+ {1.191448426478074, -3.6881232554134113},
+ {1.203076931852669, -3.709761801642497},
+ {1.2151912836117884, -3.731648724559698},
+ {1.2278141462274539, -3.753817952785642},
+ {1.2409721988621754, -3.7763071458245956},
+ {1.2546968614661016, -3.7991584885622527},
+ {1.2690252219158191, -3.82241969589366},
+ {1.2840012342034617, -3.846145301494764},
+ {1.2996772887032604, -3.870398337481948},
+ {1.316116303559539, -3.895252562382127},
+ {1.3333945626553043, -3.920795475499501},
+ {1.3516056511178856, -3.9471324882568375},
+ {1.3708660530144583, -3.974392848721103},
+ {1.3913233553973305, -4.002738316267376},
+ {1.4131687110627962, -4.032376331240737},
+ {1.436656614785, -4.063580905628138},
+ {1.4621380338543308, -4.0967276147972065},
+ {1.4901198983671067, -4.132356427437651},
+ {1.5213822452925796, -4.171295431810658},
+ {1.5572408030205178, -4.214938203914619},
+ {1.6002650085871786, -4.266002359325245},
+ {1.657096323745671, -4.331507506063609},
+ {1.7977393629734166, -4.48544594420617},
+ {1.8577014383575772, -4.546720690281509},
+ {1.8577014383575772, -5.497787143782138},
+ {1.2663322229932439, -5.497787143782138},
+ {1.2656247456808565, -5.4967516608259},
+ {1.2287034601894442, -5.445751427768824},
+ {1.1786904071656892, -5.381167803357418},
+ {1.0497616572686415, -5.233423649910944},
+ {0.9097522267255194, -5.096760177110545},
+ {0.8484431816837388, -5.043472544717165},
+ {0.8000049977037023, -5.003989210148714},
+ {0.7581818853313602, -4.9717075285444},
+ {0.7205493452982701, -4.944073122571691},
+ {0.6882849734317291, -4.921474754757885},
+ {0.6882849734317291, -2.40847516476558},
+ {0.8062364039734171, -2.2816832357742984},
+ {0.9846964491122989, -2.0749837238115147},
+ {1.080856081125841, -1.9535526052833936},
+ {1.1403399741223543, -1.8700303125904767},
+ {1.1796460643255697, -1.8073338252603661},
+ {1.206509908068604, -1.7574623871869075},
+ {1.225108933139178, -1.7160975113819317},
+ {1.237917343016644, -1.6806816402603253},
+ {1.2465009152225068, -1.6495957958330445},
+ {1.251901644035212, -1.6217619064422375},
+ {1.2548410275662123, -1.5964327471863136},
+ {1.2558349967266738, -1.5730732293972975},
+ {1.2552624664807475, -1.551289614657788},
+ {1.2534080548485127, -1.5307854126408047},
+ {1.2504896957865235, -1.5113328783804112},
+ {1.2466770509718135, -1.492754008779478},
+ {1.2421041193998992, -1.4749075280685116},
+ {1.236878076935188, -1.457679763958034},
+ {1.231085601347444, -1.4409781183381307},
+ {1.2247974811461413, -1.4247263085140511},
+ {1.2180720288351026, -1.408860841660136},
+ {1.2109576458572935, -1.3933283641188086},
+ {1.2034947755974974, -1.378083641634472},
+ {1.1957174082841977, -1.363088001457586},
+ {1.1876542532510088, -1.3483081171759035},
+ {1.179329661157153, -1.3337150510329991},
+ {1.1707643560768641, -1.3192834919003447},
+ {1.1385726725405734, -1.3512941162901886},
+ {1.1061744838535637, -1.3828092440478137},
+ {1.0736355415504857, -1.4137655512448706},
+ {1.0410203155384732, -1.444102884084807},
+ {1.0083912519665894, -1.4737649313813326},
+ {0.975808099297045, -1.5026998012630925},
+ {0.9433273192311136, -1.5308604887780404},
+ {0.941597772428203, -1.50959779639341},
+ {0.9392183822389457, -1.488861961175901},
+ {0.9362399962318921, -1.4685999567644576},
+ {0.9327074201772598, -1.448764371832554},
+ {0.9286602163651203, -1.4293126388801052},
+ {0.9241333769591611, -1.410206381002334},
+ {0.9191578939466147, -1.3914108561164176},
+ {0.9137612431353617, -1.3728944819981919},
+ {0.9079677963791273, -1.3546284285619337},
+ {0.9017991735984072, -1.3365862662768213},
+ {0.8952745440670551, -1.3187436615861219},
+ {0.888410884742945, -1.3010781117818295},
+ {0.8812232020507231, -1.2835687130679965},
+ {0.8737247224089426, -1.2661959565824437},
+ {0.8659270558807739, -1.2489415479874506},
+ {0.8578403365760008, -1.2317882469234374},
+ {0.8810761292150717, -1.2118184809610988},
+ {0.9041762360363244, -1.1914283310662528},
+ {0.9271196683064211, -1.1706361926784383},
+ {0.949885054142765, -1.1494613527499984},
+ {0.9724507572661958, -1.1279238910069835},
+ {0.9947950004998649, -1.1060445714951375},
+ {1.0168959923240788, -1.0838447258650978},
+ {1.0387320546908576, -1.0613461300367468},
+ {1.0602817502430675, -1.0385708760262022},
+ {1.0815240070740941, -1.0155412408147642},
+ {1.102438239206001, -0.9922795541836177},
+ {1.123004461055328, -0.9688080674301042},
+ {1.1432033942926907, -0.9451488248218312},
+ {1.1630165656794818, -0.9213235395370803},
+ {1.1824263946752058, -0.8973534756890716},
+ {1.2014162698440272, -0.8732593378443982},
+ {1.2199706133398625, -0.8490611692304734},
+ {1.2380749330068292, -0.8247782595916947},
+ {1.2557158618869284, -0.800429063408306},
+ {1.2728811851714203, -0.776031128944234},
+ {1.2895598548592355, -0.7516010383486005},
+ {1.3057419925890068, -0.7271543588072293},
+ {1.3214188812865908, -0.702705604531139},
+ {1.3365829464142562, -0.6782682091832445},
+ {1.351227727719687, -0.6538545081853449},
+ {1.365347842462401, -0.6294757302167044},
+ {1.3789389411433586, -0.6051419971134862},
+ {1.391997656782351, -0.5808623313043475},
+ {1.4045215487801634, -0.5566446698699925},
+ {1.4165090423718034, -0.5324958842910054},
+ {1.4279593646268474, -0.5084218049460658},
+ {1.4388724778869602, -0.4844272494383845},
+ {1.449249011452494, -0.460516053858597},
+ {1.4590901922431447, -0.4366911061340692},
+ {1.468397775065033, -0.4129543806643518},
+ {1.4771739730209745, -0.38930697349737264},
+ {1.485421388504271, -0.36574913735813025},
+ {1.4931429451209484, -0.3422803158986589},
+ {1.5003418207921726, -0.3188991765927855},
+ {1.5070213821985838, -0.2956036417497373},
+ {1.513185120641734, -0.2723909171654731},
+ {1.5188365893149296, -0.24925751796822435},
+ {1.5239793418959948, -0.22619929124396096},
+ {1.5286168722972349, -0.2032114350471563},
+ {1.5327525553319497, -0.1802885134112242},
+ {1.5363895879810432, -0.15742446697009113},
+ {1.5395309308654115, -0.1346126187862511},
+ {1.5421792494481048, -0.11184567494963411},
+ {1.5443368544016174, -0.0891157194637005},
+ {1.5460056404769755, -0.06641420286771664},
+ {1.5471870230984175, -0.043731923953761714},
+ {1.547881871775246, -0.021059003819238205},
+ {1.5480904392645911, 0.0016151486553661097},
+ {1.547812285227133, 0.024301881005710235},
+ {1.5470461928818935, 0.047013352405288866},
+ {1.5457900768723736, 0.06976260156314103},
+ {1.5440408801865422, 0.09256362934244797},
+ {1.5417944575035705, 0.11543149864415554},
+ {1.5390454417383017, 0.13838245474060726},
+ {1.5357870897759938, 0.16143407007284788},
+ {1.5320111023738603, 0.18460541860588778},
+ {1.5277074118667517, 0.20791728625864334},
+ {1.5228639295308124, 0.23139242581719505},
+ {1.5174662420569858, 0.25505586728497265},
+ {1.5114972433117844, 0.27893529808946027},
+ {1.5049366830391921, 0.30306153234932315},
+ {1.4977606078174102, 0.32746909510770755},
+ {1.4899406605544634, 0.35219695697813347},
+ {1.4814431917184283, 0.37728946847450484},
+ {1.4722281161523716, 0.40279756372788583},
+ {1.4622474200998372, 0.4287803341537376},
+ {1.4514431778273647, 0.45530712040457033},
+ {1.4397448652396179, 0.48246034696312606},
+ {1.427065639662639, 0.5103394485717625},
+ {1.4132970536897833, 0.5390664502570618},
+ {1.3983013135749314, 0.5687941401756967},
+ {1.3818995257862978, 0.5997184788509978},
+ {1.3638530549860057, 0.6320982830132342},
+ {1.3438323057823602, 0.6662881915757862},
+ {1.3213606855701236, 0.7027978462604986},
+ {1.2957042956404132, 0.7424084023365868},
+ {1.2656247456808565, 0.786433646353686},
+ {1.2287034601894442, 0.8374338794107618},
+ {1.1786904071656892, 0.902017503822168},
+ {1.0497616572686415, 1.0497616572686426},
+ {0.9097522267255194, 1.1864251300690412},
+ {0.8484431816837388, 1.2397127624624213},
+ {0.8000049977037023, 1.2791960970308718},
+ {0.7581818853313602, 1.3114777786351866},
+ {0.7205493452982701, 1.3391121846078957},
+ {0.6882849734317291, 1.3617105524217008},
+ {0.6882849734317291, 2.356194490192345},
+ {1.3376784164442164, 2.356194490192345},
+ {1.3516056511178856, 2.3360528189227487},
+ {1.3708660530144583, 2.308792458458483},
+ {1.3913233553973305, 2.2804469909122105},
+ {1.4131687110627962, 2.2508089759388485},
+ {1.436656614785, 2.219604401551449},
+ {1.4621380338543308, 2.18645769238238},
+ {1.4901198983671067, 2.1508288797419346},
+ {1.5213822452925796, 2.111889875368928},
+ {1.5572408030205178, 2.0682471032649676},
+ {1.6002650085871786, 2.0171829478543404},
+ {1.657096323745671, 1.9516778011159774},
+ {1.7977393629734166, 1.7977393629734166},
+ {1.8577014383575772, 1.7364646168980775},
+ {1.8577014383575772, -1.7353804562372057}});
+}
+
+BoundsCheck MakeClippedArmSpace() {
+ return MakeArbitraryArmSpace({{1.8577014383575772, -1.7353804562372057},
+ {1.8322288438826315, -1.761574570216062},
+ {1.7840339881582052, -1.8122752826851365},
+ {1.7367354460218392, -1.863184376466228},
+ {1.69054633425053, -1.9140647421906793},
+ {1.6456590387358871, -1.9646939485526782},
+ {1.6022412524081968, -2.0148691793459164},
+ {1.5604334435784202, -2.0644108218872432},
+ {1.5203477477575325, -2.1131646494909866},
+ {1.4820681545944325, -2.1610026706370666},
+ {1.4456517763321752, -2.207822815007094},
+ {1.4111309389855604, -2.253547685496041},
+ {1.3785158286698027, -2.298122627340264},
+ {1.3477974448369014, -2.3415133576238922},
+ {1.318950649522341, -2.3837033700108243},
+ {1.2919371475531436, -2.4246912899477153},
+ {1.266708279449626, -2.464488312575651},
+ {1.2432075513427807, -2.5031158147261996},
+ {1.2213728618107609, -2.5406031969824228},
+ {1.2011384131259828, -2.5769859833310096},
+ {1.1824363142639513, -2.61230418457078},
+ {1.165197896140849, -2.64660091671642},
+ {1.149354767212547, -2.6799212560849437},
+ {1.134839641107821, -2.712311307404836},
+ {1.121586968578607, -2.7438174590386177},
+ {1.1095334047223224, -2.774485799309971},
+ {1.09861813993779, -2.8043616692157007},
+ {1.088783119985477, -2.8334893289039758},
+ {1.079973177230731, -2.861911717797479},
+ {1.0721360919151535, -2.8896702908486835},
+ {1.065222599283597, -2.916804915950962},
+ {1.0591863556764607, -2.943353819884641},
+ {1.0539838743128325, -2.969353572295195},
+ {1.049574439440948, -2.9948390990606946},
+ {1.0459200058000553, -3.01984371800932},
+ {1.0429850888932353, -3.044399191310831},
+ {1.0407366503803421, -3.0685357900111128},
+ {1.039143981929867, -3.092282367132034},
+ {1.0381785900853944, -3.1156664365461078},
+ {1.0378140840766794, -3.1387142554815886},
+ {1.038026068010855, -3.1614509090414518},
+ {1.0387920384931424, -3.1839003955494407},
+ {1.0400912884293725, -3.2060857118856374},
+ {1.0419048175385754, -3.2280289382581637},
+ {1.0442152499395827, -3.2497513220895704},
+ {1.047006759060362, -3.2712733608874625},
+ {1.050265000044113, -3.2926148841283163},
+ {1.0513266200838918, -3.2986722862692828},
+ {0.6882849734317291, -3.2986722862692828},
+ {0.6882849734317291, -2.40847516476558},
+ {0.8062364039734171, -2.2816832357742984},
+ {0.9846964491122989, -2.0749837238115147},
+ {1.080856081125841, -1.9535526052833936},
+ {1.1403399741223543, -1.8700303125904767},
+ {1.1796460643255697, -1.8073338252603661},
+ {1.206509908068604, -1.7574623871869075},
+ {1.225108933139178, -1.7160975113819317},
+ {1.237917343016644, -1.6806816402603253},
+ {1.2465009152225068, -1.6495957958330445},
+ {1.251901644035212, -1.6217619064422375},
+ {1.2548410275662123, -1.5964327471863136},
+ {1.2558349967266738, -1.5730732293972975},
+ {1.2552624664807475, -1.551289614657788},
+ {1.2534080548485127, -1.5307854126408047},
+ {1.2504896957865235, -1.5113328783804112},
+ {1.2466770509718135, -1.492754008779478},
+ {1.2421041193998992, -1.4749075280685116},
+ {1.236878076935188, -1.457679763958034},
+ {1.231085601347444, -1.4409781183381307},
+ {1.2247974811461413, -1.4247263085140511},
+ {1.2180720288351026, -1.408860841660136},
+ {1.2109576458572935, -1.3933283641188086},
+ {1.2034947755974974, -1.378083641634472},
+ {1.1957174082841977, -1.363088001457586},
+ {1.1876542532510088, -1.3483081171759035},
+ {1.179329661157153, -1.3337150510329991},
+ {1.1707643560768641, -1.3192834919003447},
+ {1.1385726725405734, -1.3512941162901886},
+ {1.1061744838535637, -1.3828092440478137},
+ {1.0736355415504857, -1.4137655512448706},
+ {1.0410203155384732, -1.444102884084807},
+ {1.0083912519665894, -1.4737649313813326},
+ {0.975808099297045, -1.5026998012630925},
+ {0.9433273192311136, -1.5308604887780404},
+ {0.941597772428203, -1.50959779639341},
+ {0.9392183822389457, -1.488861961175901},
+ {0.9362399962318921, -1.4685999567644576},
+ {0.9327074201772598, -1.448764371832554},
+ {0.9286602163651203, -1.4293126388801052},
+ {0.9241333769591611, -1.410206381002334},
+ {0.9191578939466147, -1.3914108561164176},
+ {0.9137612431353617, -1.3728944819981919},
+ {0.9079677963791273, -1.3546284285619337},
+ {0.9017991735984072, -1.3365862662768213},
+ {0.8952745440670551, -1.3187436615861219},
+ {0.888410884742945, -1.3010781117818295},
+ {0.8812232020507231, -1.2835687130679965},
+ {0.8737247224089426, -1.2661959565824437},
+ {0.8659270558807739, -1.2489415479874506},
+ {0.8578403365760008, -1.2317882469234374},
+ {0.8810761292150717, -1.2118184809610988},
+ {0.9041762360363244, -1.1914283310662528},
+ {0.9271196683064211, -1.1706361926784383},
+ {0.949885054142765, -1.1494613527499984},
+ {0.9724507572661958, -1.1279238910069835},
+ {0.9947950004998649, -1.1060445714951375},
+ {1.0168959923240788, -1.0838447258650978},
+ {1.0387320546908576, -1.0613461300367468},
+ {1.0602817502430675, -1.0385708760262022},
+ {1.0815240070740941, -1.0155412408147642},
+ {1.102438239206001, -0.9922795541836177},
+ {1.123004461055328, -0.9688080674301042},
+ {1.1432033942926907, -0.9451488248218312},
+ {1.1630165656794818, -0.9213235395370803},
+ {1.1824263946752058, -0.8973534756890716},
+ {1.2014162698440272, -0.8732593378443982},
+ {1.2199706133398625, -0.8490611692304734},
+ {1.2380749330068292, -0.8247782595916947},
+ {1.2557158618869284, -0.800429063408306},
+ {1.2728811851714203, -0.776031128944234},
+ {1.2895598548592355, -0.7516010383486005},
+ {1.3057419925890068, -0.7271543588072293},
+ {1.3214188812865908, -0.702705604531139},
+ {1.3365829464142562, -0.6782682091832445},
+ {1.351227727719687, -0.6538545081853449},
+ {1.365347842462401, -0.6294757302167044},
+ {1.3789389411433586, -0.6051419971134862},
+ {1.391997656782351, -0.5808623313043475},
+ {1.4045215487801634, -0.5566446698699925},
+ {1.4165090423718034, -0.5324958842910054},
+ {1.4279593646268474, -0.5084218049460658},
+ {1.4388724778869602, -0.4844272494383845},
+ {1.449249011452494, -0.460516053858597},
+ {1.4590901922431447, -0.4366911061340692},
+ {1.468397775065033, -0.4129543806643518},
+ {1.4771739730209745, -0.38930697349737264},
+ {1.485421388504271, -0.36574913735813025},
+ {1.4931429451209484, -0.3422803158986589},
+ {1.5003418207921726, -0.3188991765927855},
+ {1.5070213821985838, -0.2956036417497373},
+ {1.513185120641734, -0.2723909171654731},
+ {1.5188365893149296, -0.24925751796822435},
+ {1.5239793418959948, -0.22619929124396096},
+ {1.5286168722972349, -0.2032114350471563},
+ {1.5327525553319497, -0.1802885134112242},
+ {1.5363895879810432, -0.15742446697009113},
+ {1.5395309308654115, -0.1346126187862511},
+ {1.5421792494481048, -0.11184567494963411},
+ {1.5443368544016174, -0.0891157194637005},
+ {1.5460056404769755, -0.06641420286771664},
+ {1.5471870230984175, -0.043731923953761714},
+ {1.547881871775246, -0.021059003819238205},
+ {1.5480904392645911, 0.0016151486553661097},
+ {1.547812285227133, 0.024301881005710235},
+ {1.5470461928818935, 0.047013352405288866},
+ {1.5457900768723736, 0.06976260156314103},
+ {1.5440408801865422, 0.09256362934244797},
+ {1.5417944575035705, 0.11543149864415554},
+ {1.5390454417383017, 0.13838245474060726},
+ {1.5357870897759938, 0.16143407007284788},
+ {1.5320111023738603, 0.18460541860588778},
+ {1.5277074118667517, 0.20791728625864334},
+ {1.5228639295308124, 0.23139242581719505},
+ {1.5174662420569858, 0.25505586728497265},
+ {1.5114972433117844, 0.27893529808946027},
+ {1.5049366830391921, 0.30306153234932315},
+ {1.4977606078174102, 0.32746909510770755},
+ {1.4899406605544634, 0.35219695697813347},
+ {1.4814431917184283, 0.37728946847450484},
+ {1.4722281161523716, 0.40279756372788583},
+ {1.4622474200998372, 0.4287803341537376},
+ {1.4514431778273647, 0.45530712040457033},
+ {1.4397448652396179, 0.48246034696312606},
+ {1.427065639662639, 0.5103394485717625},
+ {1.4132970536897833, 0.5390664502570618},
+ {1.3983013135749314, 0.5687941401756967},
+ {1.3818995257862978, 0.5997184788509978},
+ {1.3638530549860057, 0.6320982830132342},
+ {1.3438323057823602, 0.6662881915757862},
+ {1.3213606855701236, 0.7027978462604986},
+ {1.2957042956404132, 0.7424084023365868},
+ {1.2656247456808565, 0.786433646353686},
+ {1.2287034601894442, 0.8374338794107618},
+ {1.1786904071656892, 0.902017503822168},
+ {1.0497616572686415, 1.0497616572686426},
+ {0.9097522267255194, 1.1864251300690412},
+ {0.8484431816837388, 1.2397127624624213},
+ {0.8000049977037023, 1.2791960970308718},
+ {0.7581818853313602, 1.3114777786351866},
+ {0.7205493452982701, 1.3391121846078957},
+ {0.6882849734317291, 1.3617105524217008},
+ {0.6882849734317291, 2.356194490192345},
+ {1.3376784164442164, 2.356194490192345},
+ {1.3516056511178856, 2.3360528189227487},
+ {1.3708660530144583, 2.308792458458483},
+ {1.3913233553973305, 2.2804469909122105},
+ {1.4131687110627962, 2.2508089759388485},
+ {1.436656614785, 2.219604401551449},
+ {1.4621380338543308, 2.18645769238238},
+ {1.4901198983671067, 2.1508288797419346},
+ {1.5213822452925796, 2.111889875368928},
+ {1.5572408030205178, 2.0682471032649676},
+ {1.6002650085871786, 2.0171829478543404},
+ {1.657096323745671, 1.9516778011159774},
+ {1.7977393629734166, 1.7977393629734166},
+ {1.8577014383575772, 1.7364646168980775},
+ {1.8577014383575772, -1.7353804562372057}});
+}
+
+} // namespace control_loops
+} // namespace y2018
diff --git a/y2018/control_loops/python/arm_bounds.h b/y2018/control_loops/python/arm_bounds.h
new file mode 100644
index 0000000..b69f661
--- /dev/null
+++ b/y2018/control_loops/python/arm_bounds.h
@@ -0,0 +1,246 @@
+#ifndef Y2018_CONTORL_LOOPS_PYTHON_ARM_BOUNDS_H_
+#define Y2018_CONTORL_LOOPS_PYTHON_ARM_BOUNDS_H_
+
+#include <CGAL/Bbox_2.h>
+#include <CGAL/Boolean_set_operations_2.h>
+#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
+#include <CGAL/Polygon_2.h>
+#include <CGAL/Polygon_2_algorithms.h>
+#include <CGAL/Polygon_with_holes_2.h>
+#include <CGAL/squared_distance_2.h>
+
+#include <Eigen/Dense>
+
+// Prototype level code to find the nearest point and distance to a polygon.
+
+namespace y2018 {
+namespace control_loops {
+
+typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
+typedef K::Point_2 Point;
+typedef K::Segment_2 Segment;
+typedef CGAL::Bbox_2 Bbox;
+typedef CGAL::Polygon_2<K> SimplePolygon;
+typedef CGAL::Polygon_with_holes_2<K> Polygon;
+typedef K::Line_2 Line;
+typedef K::Vector_2 Vector;
+
+
+// Returns true if the point p3 is to the left of the vector from p1 to p2.
+inline bool is_left(Point p1, Point p2, Point p3) {
+ switch (CGAL::orientation(p1, p2, p3)) {
+ case CGAL::LEFT_TURN:
+ case CGAL::COLLINEAR:
+ return true;
+ case CGAL::RIGHT_TURN:
+ return false;
+ }
+}
+
+// Returns true if the segments intersect.
+inline bool intersects(Segment s1, Segment s2) {
+ return CGAL::do_intersect(s1, s2);
+}
+
+class BoundsCheck {
+ public:
+ BoundsCheck(const std::vector<Point> &points)
+ : points_(points), grid_(points_, 6) {}
+
+ double min_distance(Point point, ::Eigen::Matrix<double, 2, 1> *normal) const;
+
+ const std::vector<Point> &points() const { return points_; }
+
+ private:
+ static Bbox ToBbox(const std::vector<Point> &points) {
+ Bbox out;
+ out += Segment(points.back(), points.front()).bbox();
+ for (size_t i = 0; i < points.size() - 1; ++i) {
+ out += Segment(points[i], points[i + 1]).bbox();
+ }
+ return out;
+ }
+
+ static SimplePolygon ToPolygon(Bbox bbox) {
+ Point points[4]{{bbox.xmin(), bbox.ymin()},
+ {bbox.xmax(), bbox.ymin()},
+ {bbox.xmax(), bbox.ymax()},
+ {bbox.xmin(), bbox.ymax()}};
+ return SimplePolygon(&points[0], &points[4]);
+ }
+
+ static double min_dist(Point pt, const std::vector<Point> &points,
+ Segment *best_segment) {
+ *best_segment = Segment(points.back(), points.front());
+ double min_dist_sqr = CGAL::squared_distance(pt, *best_segment);
+ for (size_t i = 0; i < points.size() - 1; ++i) {
+ Segment s(points[i], points[i + 1]);
+ double segment_distance = CGAL::squared_distance(pt, s);
+ if (segment_distance < min_dist_sqr) {
+ min_dist_sqr = segment_distance;
+ *best_segment = s;
+ }
+ }
+ return sqrt(min_dist_sqr);
+ }
+
+ static std::vector<Segment> ToSegment(Bbox bbox) {
+ Point points[4]{{bbox.xmin(), bbox.ymin()},
+ {bbox.xmax(), bbox.ymin()},
+ {bbox.xmax(), bbox.ymax()},
+ {bbox.xmin(), bbox.ymax()}};
+
+ return std::vector<Segment>({{points[0], points[1]},
+ {points[1], points[2]},
+ {points[2], points[3]},
+ {points[3], points[0]}});
+ }
+
+ static bool check_inside(Point pt, const std::vector<Point> &points) {
+ switch (CGAL::bounded_side_2(&points[0], &points[points.size()], pt, K())) {
+ case CGAL::ON_BOUNDED_SIDE:
+ case CGAL::ON_BOUNDARY:
+ return true;
+ case CGAL::ON_UNBOUNDED_SIDE:
+ return false;
+ }
+ return false;
+ }
+
+ const std::vector<Point> points_;
+
+ class GridCell {
+ public:
+ GridCell(const std::vector<Point> &points, Bbox bbox) {
+ bool has_intersect = false;
+
+ Point center{(bbox.xmin() + bbox.xmax()) / 2,
+ (bbox.ymin() + bbox.ymax()) / 2};
+ // Purposefully overestimate.
+ double r = bbox.ymax() - bbox.ymin();
+
+ Segment best_segment;
+ double best = min_dist(center, points, &best_segment);
+ dist_upper_bound_ = best + 2 * r;
+ dist_lower_bound_ = std::max(best - 2 * r, 0.0);
+
+ double sq_upper_bound = dist_upper_bound_ * dist_upper_bound_;
+
+ auto try_add_segment = [&](Segment segment) {
+ for (const auto &bbox_segment : ToSegment(bbox)) {
+ if (CGAL::do_intersect(bbox_segment, segment)) {
+ has_intersect = true;
+ }
+ }
+
+ double dist_sqr = CGAL::squared_distance(center, segment);
+ if (dist_sqr < sq_upper_bound) {
+ segments_.push_back(segment);
+ }
+ };
+
+ try_add_segment(Segment(points.back(), points.front()));
+ for (size_t i = 0; i < points.size() - 1; ++i) {
+ try_add_segment(Segment(points[i], points[i + 1]));
+ }
+ if (has_intersect) {
+ is_borderline = true;
+ } else {
+ is_inside = check_inside(center, points);
+ }
+ }
+
+ bool IsInside(Point pt) const {
+ (void)pt;
+ return is_inside;
+ }
+
+ bool IsBorderline() const { return is_borderline; }
+
+ double DistanceSqr(Point pt, Segment *best_segment) const {
+ double min_dist_sqr = CGAL::squared_distance(pt, segments_[0]);
+ *best_segment = segments_[0];
+ for (size_t i = 1; i < segments_.size(); ++i) {
+ double new_distance = CGAL::squared_distance(pt, segments_[i]);
+ if (new_distance < min_dist_sqr) {
+ min_dist_sqr = new_distance;
+ *best_segment = segments_[i];
+ }
+ }
+ return min_dist_sqr;
+ }
+ double Distance(Point pt, Segment *best_segment) const {
+ return sqrt(DistanceSqr(pt, best_segment));
+ }
+
+ bool is_inside = false;
+ bool is_borderline = false;
+ double dist_upper_bound_;
+ double dist_lower_bound_;
+ std::vector<Segment> segments_;
+ std::vector<std::vector<Point>> polygons_;
+ };
+
+ class GridSystem {
+ public:
+ // Precision is really 2**-precision and must be positive.
+ GridSystem(const std::vector<Point> &points, int precision)
+ : points_(points), scale_factor_(1 << precision) {
+ auto bbox = ToBbox(points);
+ fprintf(stderr, "%g %g, %g %g\n", bbox.xmin(), bbox.ymin(), bbox.xmax(),
+ bbox.ymax());
+ x_min_ = static_cast<int>(std::floor(bbox.xmin() * scale_factor_)) - 1;
+ y_min_ = static_cast<int>(std::floor(bbox.ymin() * scale_factor_)) - 1;
+
+ stride_ = static_cast<int>(bbox.xmax() * scale_factor_) + 3 - x_min_;
+ height_ = static_cast<int>(bbox.ymax() * scale_factor_) + 3 - y_min_;
+
+ fprintf(stderr, "num_cells: %d\n", stride_ * height_);
+ cells_.reserve(stride_ * height_);
+ for (int y_cell = 0; y_cell < height_; ++y_cell) {
+ for (int x_cell = 0; x_cell < stride_; ++x_cell) {
+ cells_.push_back(
+ GridCell(points, Bbox(static_cast<double>(x_cell + x_min_) /
+ static_cast<double>(scale_factor_),
+ static_cast<double>(y_cell + y_min_) /
+ static_cast<double>(scale_factor_),
+ static_cast<double>(x_cell + x_min_ + 1) /
+ static_cast<double>(scale_factor_),
+ static_cast<double>(y_cell + y_min_ + 1) /
+ static_cast<double>(scale_factor_))));
+ }
+ }
+ }
+
+ const GridCell *GetCell(Point pt) const {
+ int x_cell =
+ static_cast<int>(std::floor(pt.x() * scale_factor_)) - x_min_;
+ int y_cell =
+ static_cast<int>(std::floor(pt.y() * scale_factor_)) - y_min_;
+ if (x_cell < 0 || x_cell >= stride_) return nullptr;
+ if (y_cell < 0 || y_cell >= height_) return nullptr;
+ return &cells_[stride_ * y_cell + x_cell];
+ }
+
+ const std::vector<Point> &points() const { return points_; }
+
+ private:
+ std::vector<Point> points_;
+ int scale_factor_;
+ int x_min_;
+ int y_min_;
+ int stride_;
+ int height_;
+ std::vector<GridCell> cells_;
+ };
+
+ GridSystem grid_;
+};
+
+BoundsCheck MakeClippedArmSpace();
+BoundsCheck MakeFullArmSpace();
+
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTORL_LOOPS_PYTHON_ARM_BOUNDS_H_
diff --git a/y2018/control_loops/python/arm_mpc.cc b/y2018/control_loops/python/arm_mpc.cc
index 2727545..a032cae 100644
--- a/y2018/control_loops/python/arm_mpc.cc
+++ b/y2018/control_loops/python/arm_mpc.cc
@@ -3,14 +3,112 @@
#include <thread>
#include <ct/optcon/optcon.h>
+#include <Eigen/Eigenvalues>
#include "third_party/gflags/include/gflags/gflags.h"
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#include "y2018/control_loops/python/arm_bounds.h"
+#include "y2018/control_loops/python/dlqr.h"
-DEFINE_double(boundary_scalar, 12.0, "Test command-line flag");
-DEFINE_double(boundary_rate, 25.0, "Sigmoid rate");
-DEFINE_bool(sigmoid, true, "If true, sigmoid, else exponential.");
+DEFINE_double(boundary_scalar, 1500.0, "Test command-line flag");
+DEFINE_double(velocity_boundary_scalar, 10.0, "Test command-line flag");
+DEFINE_double(boundary_rate, 20.0, "Sigmoid rate");
+DEFINE_bool(linear, false, "If true, linear, else see sigmoid.");
+DEFINE_bool(sigmoid, false, "If true, sigmoid, else exponential.");
DEFINE_double(round_corner, 0.0, "Corner radius of the constraint box.");
+DEFINE_double(convergance, 1e-12, "Residual before finishing the solver.");
+DEFINE_double(position_allowance, 5.0,
+ "Distance to Velocity at which we have 0 penalty conversion.");
+DEFINE_double(bounds_offset, 0.02, "Offset the quadratic boundary in by this");
+DEFINE_double(linear_bounds_offset, 0.00,
+ "Offset the linear boundary in by this");
+DEFINE_double(yrange, 1.0,
+ "+- y max for saturating out the state for the cost function.");
+DEFINE_bool(debug_print, false, "Print the debugging print from the solver.");
+DEFINE_bool(print_starting_summary, true,
+ "Print the summary on the pre-solution.");
+DEFINE_bool(print_summary, false, "Print the summary on each iteration.");
+DEFINE_bool(quadratic, true, "If true, quadratic bounds penalty.");
+
+DEFINE_bool(reset_every_cycle, false,
+ "If true, reset the initial guess every cycle.");
+
+DEFINE_double(seconds, 1.5, "The number of seconds to simulate.");
+
+DEFINE_double(theta0, 1.0, "Starting theta0");
+DEFINE_double(theta1, 0.9, "Starting theta1");
+
+DEFINE_double(goal_theta0, -0.5, "Starting theta0");
+DEFINE_double(goal_theta1, -0.5, "Starting theta1");
+
+DEFINE_double(qpos1, 0.2, "qpos1");
+DEFINE_double(qvel1, 4.0, "qvel1");
+DEFINE_double(qpos2, 0.2, "qpos2");
+DEFINE_double(qvel2, 4.0, "qvel2");
+
+DEFINE_double(u_over_linear, 0.0, "Linear penalty for too much U.");
+DEFINE_double(u_over_quadratic, 4.0, "Quadratic penalty for too much U.");
+
+DEFINE_double(time_horizon, 0.75, "MPC time horizon");
+
+DEFINE_bool(only_print_eigenvalues, false,
+ "If true, stop after computing the final eigenvalues");
+
+DEFINE_bool(plot_xy, false, "If true, plot the xy trajectory of the end of the arm.");
+DEFINE_bool(plot_cost, false, "If true, plot the cost function.");
+DEFINE_bool(plot_state_cost, false,
+ "If true, plot the state portion of the cost function.");
+DEFINE_bool(plot_states, false, "If true, plot the states.");
+DEFINE_bool(plot_u, false, "If true, plot the control signal.");
+
+static constexpr double kDt = 0.00505;
+
+namespace y2018 {
+namespace control_loops {
+
+::Eigen::Matrix<double, 4, 4> NumericalJacobianX(
+ ::Eigen::Matrix<double, 4, 1> (*fn)(
+ ::Eigen::Ref<::Eigen::Matrix<double, 4, 1>> X,
+ ::Eigen::Ref<::Eigen::Matrix<double, 2, 1>> U, double dt),
+ ::Eigen::Matrix<double, 4, 1> X, ::Eigen::Matrix<double, 2, 1> U, double dt,
+ const double kEpsilon = 1e-4) {
+ constexpr int num_states = 4;
+ ::Eigen::Matrix<double, 4, 4> answer = ::Eigen::Matrix<double, 4, 4>::Zero();
+
+ // It's more expensive, but +- epsilon will be more reliable
+ for (int i = 0; i < num_states; ++i) {
+ ::Eigen::Matrix<double, 4, 1> dX_plus = X;
+ dX_plus(i, 0) += kEpsilon;
+ ::Eigen::Matrix<double, 4, 1> dX_minus = X;
+ dX_minus(i, 0) -= kEpsilon;
+ answer.block<4, 1>(0, i) =
+ (fn(dX_plus, U, dt) - fn(dX_minus, U, dt)) / kEpsilon / 2.0;
+ }
+ return answer;
+}
+
+::Eigen::Matrix<double, 4, 2> NumericalJacobianU(
+ ::Eigen::Matrix<double, 4, 1> (*fn)(
+ ::Eigen::Ref<::Eigen::Matrix<double, 4, 1>> X,
+ ::Eigen::Ref<::Eigen::Matrix<double, 2, 1>> U, double dt),
+ ::Eigen::Matrix<double, 4, 1> X, ::Eigen::Matrix<double, 2, 1> U, double dt,
+ const double kEpsilon = 1e-4) {
+ constexpr int num_states = 4;
+ constexpr int num_inputs = 2;
+ ::Eigen::Matrix<double, num_states, num_inputs> answer =
+ ::Eigen::Matrix<double, num_states, num_inputs>::Zero();
+
+ // It's more expensive, but +- epsilon will be more reliable
+ for (int i = 0; i < num_inputs; ++i) {
+ ::Eigen::Matrix<double, 2, 1> dU_plus = U;
+ dU_plus(i, 0) += kEpsilon;
+ ::Eigen::Matrix<double, 2, 1> dU_minus = U;
+ dU_minus(i, 0) -= kEpsilon;
+ answer.block<4, 1>(0, i) =
+ (fn(X, dU_plus, dt) - fn(X, dU_minus, dt)) / kEpsilon / 2.0;
+ }
+ return answer;
+}
// This code is for analysis and simulation of a double jointed arm. It is an
// attempt to see if a MPC could work for arm control under constraints.
@@ -38,6 +136,29 @@
}
virtual ~MySecondOrderSystem() {}
+ static constexpr SCALAR l1 = 46.25 * 0.0254;
+ static constexpr SCALAR l2 = 41.80 * 0.0254;
+
+ static constexpr SCALAR m1 = 9.34 / 2.2;
+ static constexpr SCALAR m2 = 9.77 / 2.2;
+
+ static constexpr SCALAR J1 = 2957.05 * 0.0002932545454545454;
+ static constexpr SCALAR J2 = 2824.70 * 0.0002932545454545454;
+
+ static constexpr SCALAR r1 = 21.64 * 0.0254;
+ static constexpr SCALAR r2 = 26.70 * 0.0254;
+
+ static constexpr SCALAR G1 = 140.0;
+ static constexpr SCALAR G2 = 90.0;
+
+ static constexpr SCALAR stall_torque = 1.41;
+ static constexpr SCALAR free_speed = (5840.0 / 60.0) * 2.0 * M_PI;
+ static constexpr SCALAR stall_current = 89.0;
+ static constexpr SCALAR R = 12.0 / stall_current;
+
+ static constexpr SCALAR Kv = free_speed / 12.0;
+ static constexpr SCALAR Kt = stall_torque / stall_current;
+
// Evaluate the system dynamics.
//
// Args:
@@ -49,11 +170,312 @@
const ::ct::core::StateVector<4, SCALAR> &state, const SCALAR & /*t*/,
const ::ct::core::ControlVector<2, SCALAR> &control,
::ct::core::StateVector<4, SCALAR> &derivative) override {
- derivative(0) = state(1);
- derivative(1) = control(0);
- derivative(2) = state(3);
- derivative(3) = control(1);
+ derivative = Dynamics(state, control);
}
+
+ static ::Eigen::Matrix<double, 4, 1> Dynamics(
+ const ::ct::core::StateVector<4, SCALAR> &X,
+ const ::ct::core::ControlVector<2, SCALAR> &U) {
+ ::ct::core::StateVector<4, SCALAR> derivative;
+ const SCALAR alpha = J1 + r1 * r1 * m1 + l1 * l1 * m2;
+ const SCALAR beta = l1 * r2 * m2;
+ const SCALAR gamma = J2 + r2 * r2 * m2;
+
+ const SCALAR s = sin(X(0) - X(2));
+ const SCALAR c = cos(X(0) - X(2));
+
+ // K1 * d^2 theta/dt^2 + K2 * d theta/dt = torque
+ ::Eigen::Matrix<SCALAR, 2, 2> K1;
+ K1(0, 0) = alpha;
+ K1(1, 0) = K1(0, 1) = c * beta;
+ K1(1, 1) = gamma;
+
+ ::Eigen::Matrix<SCALAR, 2, 2> K2 = ::Eigen::Matrix<SCALAR, 2, 2>::Zero();
+ K2(0, 1) = s * beta * X(3);
+ K2(1, 0) = -s * beta * X(1);
+
+ const SCALAR kNumDistalMotors = 2.0;
+ ::Eigen::Matrix<SCALAR, 2, 1> torque;
+ torque(0, 0) = G1 * (U(0) * Kt / R - X(1) * G1 * Kt / (Kv * R));
+ torque(1, 0) = G2 * (U(1) * kNumDistalMotors * Kt / R -
+ X(3) * G2 * Kt * kNumDistalMotors / (Kv * R));
+
+ ::Eigen::Matrix<SCALAR, 2, 1> velocity;
+ velocity(0, 0) = X(0);
+ velocity(1, 0) = X(2);
+
+ const ::Eigen::Matrix<SCALAR, 2, 1> accel =
+ K1.inverse() * (torque - K2 * velocity);
+
+ derivative(0) = X(1);
+ derivative(1) = accel(0);
+ derivative(2) = X(3);
+ derivative(3) = accel(1);
+
+ return derivative;
+ }
+
+ // Runge-Kutta.
+ static ::Eigen::Matrix<double, 4, 1> DiscreteDynamics(
+ ::Eigen::Ref<::Eigen::Matrix<double, 4, 1>> X,
+ ::Eigen::Ref<::Eigen::Matrix<double, 2, 1>> U, double dt) {
+ const double half_dt = dt * 0.5;
+ ::Eigen::Matrix<double, 4, 1> k1 = Dynamics(X, U);
+ ::Eigen::Matrix<double, 4, 1> k2 = Dynamics(X + half_dt * k1, U);
+ ::Eigen::Matrix<double, 4, 1> k3 = Dynamics(X + half_dt * k2, U);
+ ::Eigen::Matrix<double, 4, 1> k4 = Dynamics(X + dt * k3, U);
+ return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+ }
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double,
+ typename SCALAR = SCALAR_EVAL>
+class ObstacleAwareQuadraticCost
+ : public ::ct::optcon::TermBase<STATE_DIM, CONTROL_DIM, SCALAR_EVAL,
+ SCALAR> {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> state_vector_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM>
+ control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM>
+ state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM>
+ control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM>
+ control_state_matrix_double_t;
+
+ ObstacleAwareQuadraticCost(const ::Eigen::Matrix<double, 2, 2> &R,
+ const ::Eigen::Matrix<double, 4, 4> &Q)
+ : R_(R), Q_(Q) {}
+
+ ObstacleAwareQuadraticCost(const ObstacleAwareQuadraticCost &arg)
+ : R_(arg.R_), Q_(arg.Q_) {}
+ static constexpr double kEpsilon = 1.0e-5;
+
+ virtual ~ObstacleAwareQuadraticCost() {}
+
+ ObstacleAwareQuadraticCost<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+ *clone() const override {
+ return new ObstacleAwareQuadraticCost(*this);
+ }
+
+ double SaturateX(double x, double yrange) {
+ return 2.0 * ((1.0 / (1.0 + ::std::exp(-x * 2.0 / yrange)) - 0.5)) * yrange;
+ }
+
+ SCALAR distance(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & /*u*/) {
+ constexpr double kCornerNewUpper0 = 0.35;
+ // constexpr double kCornerUpper1 = 3.13;
+ // Push it up a bit further (non-real) until we have an actual path cost.
+ constexpr double kCornerNewUpper1 = 3.39;
+ constexpr double kCornerNewUpper0_far = 10.0;
+
+ // Push it up a bit further (non-real) until we have an actual path cost.
+ // constexpr double kCornerUpper0 = 0.315;
+ constexpr double kCornerUpper0 = 0.310;
+ // constexpr double kCornerUpper1 = 3.13;
+ constexpr double kCornerUpper1 = 3.25;
+ constexpr double kCornerUpper0_far = 10.0;
+
+ constexpr double kCornerLower0 = 0.023;
+ constexpr double kCornerLower1 = 1.57;
+ constexpr double kCornerLower0_far = 10.0;
+
+ const Segment new_upper_segment(
+ Point(kCornerNewUpper0, kCornerNewUpper1),
+ Point(kCornerNewUpper0_far, kCornerNewUpper1));
+ const Segment upper_segment(Point(kCornerUpper0, kCornerUpper1),
+ Point(kCornerUpper0_far, kCornerUpper1));
+ const Segment lower_segment(Point(kCornerLower0, kCornerLower1),
+ Point(kCornerLower0_far, kCornerLower1));
+
+ Point current_point(x(0, 0), x(2, 0));
+
+ SCALAR result = 0.0;
+ if (intersects(new_upper_segment,
+ Segment(current_point,
+ Point(FLAGS_goal_theta0, FLAGS_goal_theta1)))) {
+ result += hypot(current_point.x() - kCornerNewUpper0,
+ current_point.y() - kCornerNewUpper1);
+ current_point = Point(kCornerNewUpper0, kCornerNewUpper1);
+ }
+
+ if (intersects(upper_segment,
+ Segment(current_point,
+ Point(FLAGS_goal_theta0, FLAGS_goal_theta1)))) {
+ result += hypot(current_point.x() - kCornerUpper0,
+ current_point.y() - kCornerUpper1);
+ current_point = Point(kCornerUpper0, kCornerUpper1);
+ }
+
+ if (intersects(lower_segment,
+ Segment(current_point,
+ Point(FLAGS_goal_theta0, FLAGS_goal_theta1)))) {
+ result += hypot(current_point.x() - kCornerLower0,
+ current_point.y() - kCornerLower1);
+ current_point = Point(kCornerLower0, kCornerLower1);
+ }
+ result += hypot(current_point.x() - FLAGS_goal_theta0,
+ current_point.y() - FLAGS_goal_theta1);
+ return result;
+ }
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> &u,
+ const SCALAR & /*t*/) override {
+ // Positive means violation.
+ Eigen::Matrix<SCALAR, STATE_DIM, 1> saturated_x = x;
+ SCALAR d = distance(x, u);
+ saturated_x(0, 0) = d;
+ saturated_x(2, 0) = 0.0;
+
+ saturated_x(0, 0) = SaturateX(saturated_x(0, 0), FLAGS_yrange);
+ saturated_x(2, 0) = 0.0;
+
+ //SCALAR saturation_scalar = saturated_x(0, 0) / d;
+ //saturated_x(1, 0) *= saturation_scalar;
+ //saturated_x(3, 0) *= saturation_scalar;
+
+ SCALAR result = (saturated_x.transpose() * Q_ * saturated_x +
+ u.transpose() * R_ * u)(0, 0);
+
+ if (::std::abs(u(0, 0)) > 11.0) {
+ result += (::std::abs(u(0, 0)) - 11.0) * FLAGS_u_over_linear;
+ result += (::std::abs(u(0, 0)) - 11.0) * (::std::abs(u(0, 0)) - 11.0) *
+ FLAGS_u_over_quadratic;
+ }
+ if (::std::abs(u(1, 0)) > 11.0) {
+ result += (::std::abs(u(1, 0)) - 11.0) * FLAGS_u_over_linear;
+ result += (::std::abs(u(1, 0)) - 11.0) * (::std::abs(u(1, 0)) - 11.0) *
+ FLAGS_u_over_quadratic;
+ }
+ return result;
+ }
+
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override {
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> result =
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL>::Zero();
+
+ // Perterb x for both position axis and return the result.
+ for (size_t i = 0; i < STATE_DIM; i += 1) {
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> plus_perterbed_x = x;
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> minus_perterbed_x = x;
+ plus_perterbed_x[i] += epsilon;
+ minus_perterbed_x[i] -= epsilon;
+ result[i] = (evaluate(plus_perterbed_x, u, t) -
+ evaluate(minus_perterbed_x, u, t)) /
+ (epsilon * 2.0);
+ }
+ return result;
+ }
+
+ // Compute second order derivative of this cost term w.r.t. the state
+ state_matrix_t stateSecondDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override {
+ state_matrix_t result = state_matrix_t::Zero();
+
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ // Perterb x a second time.
+ for (size_t i = 0; i < STATE_DIM; i += 1) {
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> plus_perterbed_x = x;
+ ct::core::StateVector<STATE_DIM, SCALAR_EVAL> minus_perterbed_x = x;
+ plus_perterbed_x[i] += epsilon;
+ minus_perterbed_x[i] -= epsilon;
+ state_vector_t delta = (stateDerivative(plus_perterbed_x, u, t) -
+ stateDerivative(minus_perterbed_x, u, t)) /
+ (epsilon * 2.0);
+
+ result.col(i) = delta;
+ }
+ //::std::cout << "Q_numeric " << result << " endQ" << ::std::endl;
+ return result;
+ }
+
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override {
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> result =
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL>::Zero();
+
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ // Perterb x a second time.
+ for (size_t i = 0; i < CONTROL_DIM; i += 1) {
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> plus_perterbed_u = u;
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> minus_perterbed_u = u;
+ plus_perterbed_u[i] += epsilon;
+ minus_perterbed_u[i] -= epsilon;
+ SCALAR delta = (evaluate(x, plus_perterbed_u, t) -
+ evaluate(x, minus_perterbed_u, t)) /
+ (epsilon * 2.0);
+
+ result[i] = delta;
+ }
+ //::std::cout << "cd " << result(0, 0) << " " << result(1, 0) << " endcd"
+ //<< ::std::endl;
+
+ return result;
+ }
+
+ control_state_matrix_t stateControlDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL> & /*x*/,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> & /*u*/,
+ const SCALAR_EVAL & /*t*/) override {
+ // No coupling here, so let's not bother to calculate it.
+ control_state_matrix_t result = control_state_matrix_t::Zero();
+ return result;
+ }
+
+ control_matrix_t controlSecondDerivative(
+ const ct::core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override {
+ control_matrix_t result = control_matrix_t::Zero();
+
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ //static int j = 0;
+ //::std::this_thread::sleep_for(::std::chrono::milliseconds(j % 10));
+ //int k = ++j;
+ // Perterb x a second time.
+ for (size_t i = 0; i < CONTROL_DIM; i += 1) {
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> plus_perterbed_u = u;
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> minus_perterbed_u = u;
+ plus_perterbed_u[i] += epsilon;
+ minus_perterbed_u[i] -= epsilon;
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> delta =
+ (controlDerivative(x, plus_perterbed_u, t) -
+ controlDerivative(x, minus_perterbed_u, t)) /
+ (epsilon * 2.0);
+
+ //::std::cout << "delta: " << delta(0, 0) << " " << delta(1, 0) << " k "
+ //<< k << ::std::endl;
+ result.col(i) = delta;
+ }
+ //::std::cout << "R_numeric " << result << " endR 0.013888888888888888 k:" << k
+ //<< ::std::endl;
+ //::std::cout << "x " << x << " u " << u << " k " << k << ::std::endl;
+
+ return result;
+ }
+
+ private:
+ const ::Eigen::Matrix<double, 2, 2> R_;
+ const ::Eigen::Matrix<double, 4, 4> Q_;
};
template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double,
@@ -75,11 +497,12 @@
typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM>
control_state_matrix_double_t;
- MyTermStateBarrier() {}
+ MyTermStateBarrier(BoundsCheck *bounds_check) : bounds_check_(bounds_check) {}
- MyTermStateBarrier(const MyTermStateBarrier & /*arg*/) {}
+ MyTermStateBarrier(const MyTermStateBarrier &arg)
+ : bounds_check_(arg.bounds_check_) {}
- static constexpr double kEpsilon = 1.0e-7;
+ static constexpr double kEpsilon = 5.0e-6;
virtual ~MyTermStateBarrier() {}
@@ -88,36 +511,57 @@
return new MyTermStateBarrier(*this);
}
+ SCALAR distance(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & /*u*/,
+ const SCALAR & /*t*/, Eigen::Matrix<SCALAR, 2, 1> *norm) {
+ return bounds_check_->min_distance(Point(x(0, 0), x(2, 0)), norm);
+ }
+
virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
- const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & /*u*/,
- const SCALAR & /*t*/) override {
- SCALAR min_distance;
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & u,
+ const SCALAR & t) override {
+ Eigen::Matrix<SCALAR, 2, 1> norm = Eigen::Matrix<SCALAR, 2, 1>::Zero();
+ SCALAR min_distance = distance(x, u, t, &norm);
- // Round the corner by this amount.
- SCALAR round_corner = SCALAR(FLAGS_round_corner);
+ // Velocity component (+) towards the wall.
+ SCALAR velocity_penalty = -(x(1, 0) * norm(0, 0) + x(3, 0) * norm(1, 0));
+ if (min_distance + FLAGS_bounds_offset < 0.0) {
+ velocity_penalty = 0.0;
+ }
- // Positive means violation.
- SCALAR theta0_distance = x(0, 0) - (0.5 + round_corner);
- SCALAR theta1_distance = (0.8 - round_corner) - x(2, 0);
- if (theta0_distance < SCALAR(0.0) && theta1_distance < SCALAR(0.0)) {
- // Ok, both outside. Return corner distance.
- min_distance = -hypot(theta1_distance, theta0_distance);
- } else if (theta0_distance < SCALAR(0.0) && theta1_distance > SCALAR(0.0)) {
- min_distance = theta0_distance;
- } else if (theta0_distance > SCALAR(0.0) && theta1_distance < SCALAR(0.0)) {
- min_distance = theta1_distance;
- } else {
- min_distance = ::std::min(theta0_distance, theta1_distance);
- }
- min_distance += round_corner;
- if (FLAGS_sigmoid) {
- return FLAGS_boundary_scalar /
- (1.0 + ::std::exp(-min_distance * FLAGS_boundary_rate));
- } else {
- // Values of 4 and 15 work semi resonably.
- return FLAGS_boundary_scalar *
- ::std::exp(min_distance * FLAGS_boundary_rate);
- }
+ SCALAR result;
+ //if (FLAGS_quadratic) {
+ result = 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_linear_bounds_offset) +
+ FLAGS_velocity_boundary_scalar *
+ ::std::max(0.0, min_distance + FLAGS_linear_bounds_offset) *
+ ::std::max(0.0, velocity_penalty) *
+ ::std::max(0.0, velocity_penalty);
+ /*
+} else if (FLAGS_linear) {
+result =
+FLAGS_boundary_scalar * ::std::max(0.0, min_distance) +
+FLAGS_velocity_boundary_scalar * ::std::max(0.0, -velocity_penalty);
+} else if (FLAGS_sigmoid) {
+result = FLAGS_boundary_scalar /
+ (1.0 + ::std::exp(-min_distance * FLAGS_boundary_rate)) +
+FLAGS_velocity_boundary_scalar /
+ (1.0 + ::std::exp(-velocity_penalty * FLAGS_boundary_rate));
+} else {
+// Values of 4 and 15 work semi resonably.
+result = FLAGS_boundary_scalar *
+ ::std::exp(min_distance * FLAGS_boundary_rate) +
+FLAGS_velocity_boundary_scalar *
+ ::std::exp(velocity_penalty * FLAGS_boundary_rate);
+}
+if (result < 0.0) {
+printf("Result negative %f\n", result);
+}
+*/
+ return result;
}
::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(
@@ -152,7 +596,7 @@
SCALAR epsilon = SCALAR(kEpsilon);
// Perturb x a second time.
- for (size_t i = 0; i < STATE_DIM; i += 2) {
+ for (size_t i = 0; i < STATE_DIM; i += 1) {
::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> plus_perterbed_x = x;
::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> minus_perterbed_x = x;
plus_perterbed_x[i] += epsilon;
@@ -202,11 +646,14 @@
return c;
}
*/
+
+ BoundsCheck *bounds_check_;
};
-int main(int argc, char **argv) {
- gflags::ParseCommandLineFlags(&argc, &argv, false);
- // PRELIMINIARIES, see example NLOC.cpp
+int Main() {
+ // PRELIMINIARIES
+ BoundsCheck arm_space = MakeClippedArmSpace();
+
constexpr size_t state_dim = MySecondOrderSystem<double>::STATE_DIM;
constexpr size_t control_dim = MySecondOrderSystem<double>::CONTROL_DIM;
@@ -217,32 +664,64 @@
ad_linearizer(new ::ct::core::SystemLinearizer<state_dim, control_dim>(
oscillator_dynamics));
- constexpr double kQPos = 0.5;
- constexpr double kQVel = 1.65;
+ const double kQPos1 = FLAGS_qpos1;
+ const double kQVel1 = FLAGS_qvel1;
+ const double kQPos2 = FLAGS_qpos2;
+ const double kQVel2 = FLAGS_qvel2;
+
::Eigen::Matrix<double, 4, 4> Q_step;
- Q_step << 1.0 / (kQPos * kQPos), 0.0, 0.0, 0.0, 0.0, 1.0 / (kQVel * kQVel),
- 0.0, 0.0, 0.0, 0.0, 1.0 / (kQPos * kQPos), 0.0, 0.0, 0.0, 0.0,
- 1.0 / (kQVel * kQVel);
+ Q_step << 1.0 / (kQPos1 * kQPos1), 0.0, 0.0, 0.0, 0.0,
+ 1.0 / (kQVel1 * kQVel1), 0.0, 0.0, 0.0, 0.0, 1.0 / (kQPos2 * kQPos2), 0.0,
+ 0.0, 0.0, 0.0, 1.0 / (kQVel2 * kQVel2);
::Eigen::Matrix<double, 2, 2> R_step;
R_step << 1.0 / (12.0 * 12.0), 0.0, 0.0, 1.0 / (12.0 * 12.0);
- ::std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>>
- intermediate_cost(new ::ct::optcon::TermQuadratic<state_dim, control_dim>(
- Q_step, R_step));
+ ::std::shared_ptr<::ct::optcon::TermQuadratic<state_dim, control_dim>>
+ quadratic_intermediate_cost(
+ new ::ct::optcon::TermQuadratic<state_dim, control_dim>(Q_step,
+ R_step));
+ // TODO(austin): Move back to this with the new Q and R
+ ::std::shared_ptr<ObstacleAwareQuadraticCost<state_dim, control_dim>>
+ intermediate_cost(new ObstacleAwareQuadraticCost<4, 2>(R_step, Q_step));
- // TODO(austin): DARE for these.
- ::Eigen::Matrix<double, 4, 4> Q_final = Q_step;
- ::Eigen::Matrix<double, 2, 2> R_final = R_step;
+ ::Eigen::Matrix<double, 4, 4> final_A =
+ NumericalJacobianX(MySecondOrderSystem<double>::DiscreteDynamics,
+ Eigen::Matrix<double, 4, 1>::Zero(),
+ Eigen::Matrix<double, 2, 1>::Zero(), kDt);
+
+ ::Eigen::Matrix<double, 4, 2> final_B =
+ NumericalJacobianU(MySecondOrderSystem<double>::DiscreteDynamics,
+ Eigen::Matrix<double, 4, 1>::Zero(),
+ Eigen::Matrix<double, 2, 1>::Zero(), kDt);
+
+ ::Eigen::Matrix<double, 4, 4> S_lqr;
+ ::Eigen::Matrix<double, 2, 4> K_lqr;
+ ::frc971::controls::dlqr(final_A, final_B, Q_step, R_step, &K_lqr, &S_lqr);
+ ::std::cout << "A -> " << ::std::endl << final_A << ::std::endl;
+ ::std::cout << "B -> " << ::std::endl << final_B << ::std::endl;
+ ::std::cout << "K -> " << ::std::endl << K_lqr << ::std::endl;
+ ::std::cout << "S -> " << ::std::endl << S_lqr << ::std::endl;
+ ::std::cout << "Q -> " << ::std::endl << Q_step << ::std::endl;
+ ::std::cout << "R -> " << ::std::endl << R_step << ::std::endl;
+ ::std::cout << "Eigenvalues: " << (final_A - final_B * K_lqr).eigenvalues()
+ << ::std::endl;
+
+ ::Eigen::Matrix<double, 4, 4> Q_final = 0.5 * S_lqr;
+ ::Eigen::Matrix<double, 2, 2> R_final = ::Eigen::Matrix<double, 2, 2>::Zero();
::std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>>
final_cost(new ::ct::optcon::TermQuadratic<state_dim, control_dim>(
Q_final, R_final));
+ if (FLAGS_only_print_eigenvalues) {
+ return 0;
+ }
- ::std::shared_ptr<ct::optcon::TermBase<state_dim, control_dim>> bounds_cost(
- new MyTermStateBarrier<4, 2>());
+ ::std::shared_ptr<MyTermStateBarrier<state_dim, control_dim>> bounds_cost(
+ new MyTermStateBarrier<4, 2>(&arm_space));
// TODO(austin): Cost function needs constraints.
::std::shared_ptr<::ct::optcon::CostFunctionQuadratic<state_dim, control_dim>>
cost_function(
new ::ct::optcon::CostFunctionAnalytical<state_dim, control_dim>());
+ //cost_function->addIntermediateTerm(quadratic_intermediate_cost);
cost_function->addIntermediateTerm(intermediate_cost);
cost_function->addIntermediateTerm(bounds_cost);
cost_function->addFinalTerm(final_cost);
@@ -254,8 +733,8 @@
Eigen::VectorXd u_ub(control_dim);
u_ub.setConstant(12.0);
u_lb = -u_ub;
- ::std::cout << "uub " << u_ub << ::std::endl;
- ::std::cout << "ulb " << u_lb << ::std::endl;
+ //::std::cout << "uub " << u_ub << ::std::endl;
+ //::std::cout << "ulb " << u_lb << ::std::endl;
// constraint terms
std::shared_ptr<::ct::optcon::ControlInputConstraint<state_dim, control_dim>>
@@ -275,42 +754,45 @@
// Starting point.
::ct::core::StateVector<state_dim> x0;
- x0 << 1.0, 0.0, 0.9, 0.0;
+ x0 << FLAGS_theta0, 0.0, FLAGS_theta1, 0.0;
- constexpr ::ct::core::Time kTimeHorizon = 1.5;
+ const ::ct::core::Time kTimeHorizon = FLAGS_time_horizon;
::ct::optcon::OptConProblem<state_dim, control_dim> opt_con_problem(
kTimeHorizon, x0, oscillator_dynamics, cost_function, ad_linearizer);
::ct::optcon::NLOptConSettings ilqr_settings;
- ilqr_settings.dt = 0.00505; // the control discretization in [sec]
+ ilqr_settings.nThreads = 4;
+ ilqr_settings.dt = kDt; // the control discretization in [sec]
ilqr_settings.integrator = ::ct::core::IntegrationType::RK4;
+ ilqr_settings.debugPrint = FLAGS_debug_print;
ilqr_settings.discretization =
::ct::optcon::NLOptConSettings::APPROXIMATION::FORWARD_EULER;
// ilqr_settings.discretization =
// NLOptConSettings::APPROXIMATION::MATRIX_EXPONENTIAL;
- ilqr_settings.max_iterations = 20;
- ilqr_settings.min_cost_improvement = 1.0e-11;
+ ilqr_settings.max_iterations = 40;
+ ilqr_settings.min_cost_improvement = FLAGS_convergance;
ilqr_settings.nlocp_algorithm =
- ::ct::optcon::NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ //::ct::optcon::NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ::ct::optcon::NLOptConSettings::NLOCP_ALGORITHM::GNMS;
// the LQ-problems are solved using a custom Gauss-Newton Riccati solver
- // ilqr_settings.lqocp_solver =
- // NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
ilqr_settings.lqocp_solver =
- ::ct::optcon::NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER;
- ilqr_settings.printSummary = true;
+ ::ct::optcon::NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
+ //ilqr_settings.lqocp_solver =
+ //::ct::optcon::NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER;
+ ilqr_settings.printSummary = FLAGS_print_starting_summary;
if (ilqr_settings.lqocp_solver ==
::ct::optcon::NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER) {
- opt_con_problem.setBoxConstraints(box_constraints);
+ //opt_con_problem.setBoxConstraints(box_constraints);
}
- size_t K = ilqr_settings.computeK(kTimeHorizon);
- printf("Using %d steps\n", static_cast<int>(K));
+ const size_t num_steps = ilqr_settings.computeK(kTimeHorizon);
+ printf("Using %d steps\n", static_cast<int>(num_steps));
// Vector of feeback matricies.
::ct::core::FeedbackArray<state_dim, control_dim> u0_fb(
- K, ::ct::core::FeedbackMatrix<state_dim, control_dim>::Zero());
+ num_steps, ::ct::core::FeedbackMatrix<state_dim, control_dim>::Zero());
::ct::core::ControlVectorArray<control_dim> u0_ff(
- K, ::ct::core::ControlVector<control_dim>::Zero());
- ::ct::core::StateVectorArray<state_dim> x_ref_init(K + 1, x0);
+ num_steps, ::ct::core::ControlVector<control_dim>::Zero());
+ ::ct::core::StateVectorArray<state_dim> x_ref_init(num_steps + 1, x0);
::ct::core::StateFeedbackController<state_dim, control_dim>
initial_controller(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
@@ -336,9 +818,9 @@
// 1) settings for the iLQR instance used in MPC. Of course, we use the same
// settings as for solving the initial problem ...
::ct::optcon::NLOptConSettings ilqr_settings_mpc = ilqr_settings;
- ilqr_settings_mpc.max_iterations = 20;
+ ilqr_settings_mpc.max_iterations = 40;
// and we limited the printouts, too.
- ilqr_settings_mpc.printSummary = false;
+ ilqr_settings_mpc.printSummary = FLAGS_print_summary;
// 2) settings specific to model predictive control. For a more detailed
// description of those, visit ct/optcon/mpc/MpcSettings.h
::ct::optcon::mpc_settings mpc_settings;
@@ -368,7 +850,7 @@
///
auto start_time = ::std::chrono::high_resolution_clock::now();
// limit the maximum number of runs in this example
- size_t maxNumRuns = 400;
+ size_t maxNumRuns = FLAGS_seconds / kDt;
::std::cout << "Starting to run MPC" << ::std::endl;
::std::vector<double> time_array;
@@ -380,6 +862,11 @@
::std::vector<double> u0_array;
::std::vector<double> u1_array;
+ ::std::vector<double> x_array;
+ ::std::vector<double> y_array;
+
+ // TODO(austin): Plot x, y of the end of the arm.
+
for (size_t i = 0; i < maxNumRuns; i++) {
::std::cout << "Solving iteration " << i << ::std::endl;
// Time which has passed since start of MPC
@@ -389,6 +876,25 @@
::std::chrono::duration_cast<::std::chrono::microseconds>(current_time -
start_time)
.count();
+ {
+ if (FLAGS_reset_every_cycle) {
+ ::ct::core::FeedbackArray<state_dim, control_dim> u0_fb(
+ num_steps,
+ ::ct::core::FeedbackMatrix<state_dim, control_dim>::Zero());
+ ::ct::core::ControlVectorArray<control_dim> u0_ff(
+ num_steps, ::ct::core::ControlVector<control_dim>::Zero());
+ ::ct::core::StateVectorArray<state_dim> x_ref_init(num_steps + 1, x0);
+ ::ct::core::StateFeedbackController<state_dim, control_dim>
+ resolved_controller(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+ iLQR.setInitialGuess(initial_controller);
+ // we solve the optimal control problem and retrieve the solution
+ iLQR.solve();
+ resolved_controller = iLQR.getSolution();
+ ilqr_mpc.setInitialGuess(resolved_controller);
+ }
+ }
+
// prepare mpc iteration
ilqr_mpc.prepareIteration(t);
// new optimal policy
@@ -402,6 +908,9 @@
::std::chrono::duration_cast<::std::chrono::microseconds>(current_time -
start_time)
.count();
+ // TODO(austin): This is only iterating once... I need to fix that...
+ // NLOptConSolver::solve() runs for upto N iterations. This call runs
+ // runIteration() effectively once. (nlocAlgorithm_ is iLQR)
bool success = ilqr_mpc.finishIteration(x0, t, *newPolicy, ts_newPolicy);
// we break the loop in case the time horizon is reached or solve() failed
if (ilqr_mpc.timeHorizonReached() | !success) break;
@@ -427,28 +936,95 @@
ilqr_mpc.doForwardIntegration(0.0, ilqr_settings.dt, x0, newPolicy);
::std::cout << "Next X: " << x0.transpose() << ::std::endl;
+ x_array.push_back(MySecondOrderSystem<double>::l1 * sin(x0(0)) +
+ MySecondOrderSystem<double>::r2 * sin(x0(2)));
+ y_array.push_back(MySecondOrderSystem<double>::l1 * cos(x0(0)) +
+ MySecondOrderSystem<double>::r2 * cos(x0(2)));
+
// TODO(austin): Re-use the policy. Maybe? Or maybe mpc already does that.
}
// The summary contains some statistical data about time delays, etc.
ilqr_mpc.printMpcSummary();
- // Now plot our simulation.
- matplotlibcpp::plot(time_array, theta1_array, {{"label", "theta1"}});
- matplotlibcpp::plot(time_array, omega1_array, {{"label", "omega1"}});
- matplotlibcpp::plot(time_array, theta2_array, {{"label", "theta2"}});
- matplotlibcpp::plot(time_array, omega2_array, {{"label", "omega2"}});
- matplotlibcpp::legend();
+ if (FLAGS_plot_states) {
+ // Now plot our simulation.
+ matplotlibcpp::plot(time_array, theta1_array, {{"label", "theta1"}});
+ matplotlibcpp::plot(time_array, omega1_array, {{"label", "omega1"}});
+ matplotlibcpp::plot(time_array, theta2_array, {{"label", "theta2"}});
+ matplotlibcpp::plot(time_array, omega2_array, {{"label", "omega2"}});
+ matplotlibcpp::legend();
+ }
+
+ if (FLAGS_plot_xy) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(x_array, y_array, {{"label", "xy trajectory"}});
+ matplotlibcpp::legend();
+ }
+
+ if (FLAGS_plot_u) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(time_array, u0_array, {{"label", "u0"}});
+ matplotlibcpp::plot(time_array, u1_array, {{"label", "u1"}});
+ 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_state_z;
+
+ 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_state_z_row;
+
+ for (double y_coordinate = -1.0; y_coordinate < 6.0; y_coordinate += 0.05) {
+ cost_x_row.push_back(x_coordinate);
+ cost_y_row.push_back(y_coordinate);
+ Eigen::Matrix<double, 4, 1> state_matrix;
+ state_matrix << x_coordinate, 0.0, y_coordinate, 0.0;
+ Eigen::Matrix<double, 2, 1> u_matrix =
+ Eigen::Matrix<double, 2, 1>::Zero();
+ cost_state_z_row.push_back(
+ intermediate_cost->distance(state_matrix, u_matrix));
+ cost_z_row.push_back(
+ ::std::min(bounds_cost->evaluate(state_matrix, u_matrix, 0.0), 50.0));
+ }
+ cost_x.push_back(cost_x_row);
+ cost_y.push_back(cost_y_row);
+ cost_z.push_back(cost_z_row);
+ cost_state_z.push_back(cost_state_z_row);
+ }
+
+ if (FLAGS_plot_cost) {
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_z);
+ }
+
+ if (FLAGS_plot_state_cost) {
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_state_z);
+ }
matplotlibcpp::figure();
matplotlibcpp::plot(theta1_array, theta2_array, {{"label", "trajectory"}});
- ::std::vector<double> box_x{0.5, 0.5, 1.0, 1.0};
- ::std::vector<double> box_y{0.0, 0.8, 0.8, 0.0};
- matplotlibcpp::plot(box_x, box_y, {{"label", "keepout zone"}});
+ ::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", "allowed region"}});
matplotlibcpp::legend();
- matplotlibcpp::figure();
- matplotlibcpp::plot(time_array, u0_array, {{"label", "u0"}});
- matplotlibcpp::plot(time_array, u1_array, {{"label", "u1"}});
- matplotlibcpp::legend();
matplotlibcpp::show();
+
+ return 0;
+}
+
+} // namespace control_loops
+} // namespace y2018
+
+int main(int argc, char **argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
+ return ::y2018::control_loops::Main();
}
diff --git a/y2018/control_loops/python/dlqr.h b/y2018/control_loops/python/dlqr.h
new file mode 100644
index 0000000..e4411ab
--- /dev/null
+++ b/y2018/control_loops/python/dlqr.h
@@ -0,0 +1,100 @@
+#ifndef FRC971_CONTROL_LOOPS_DLQR_H_
+#define FRC971_CONTROL_LOOPS_DLQR_H_
+
+#include <Eigen/Dense>
+
+
+namespace frc971 {
+namespace controls {
+
+extern "C" {
+// Forward declaration for slicot fortran library.
+void sb02od_(char *DICO, char *JOBB, char *FACT, char *UPLO, char *JOBL,
+ char *SORT, int *N, int *M, int *P, double *A, int *LDA, double *B,
+ int *LDB, double *Q, int *LDQ, double *R, int *LDR, double *L,
+ int *LDL, double *RCOND, double *X, int *LDX, double *ALFAR,
+ double *ALFAI, double *BETA, double *S, int *LDS, double *T,
+ int *LDT, double *U, int *LDU, double *TOL, int *IWORK,
+ double *DWORK, int *LDWORK, int *BWORK, int *INFO);
+}
+
+template <int kN, int kM>
+void dlqr(::Eigen::Matrix<double, kN, kN> A, ::Eigen::Matrix<double, kN, kM> B,
+ ::Eigen::Matrix<double, kN, kN> Q, ::Eigen::Matrix<double, kM, kM> R,
+ ::Eigen::Matrix<double, kM, kN> *K,
+ ::Eigen::Matrix<double, kN, kN> *S) {
+ // Discrete (not continuous)
+ char DICO = 'D';
+ // B and R are provided instead of G.
+ char JOBB = 'B';
+ // Not factored, Q and R are provide.
+ char FACT = 'N';
+ // Store the upper triangle of Q and R.
+ char UPLO = 'U';
+ // L is zero.
+ char JOBL = 'Z';
+ // Stable eigenvalues first in the sort order
+ char SORT = 'S';
+
+ int N = 4;
+ int M = 2;
+ // Not needed since FACT = N
+ int P = 0;
+
+ int LDL = 1;
+
+ double RCOND = 0.0;
+ ::Eigen::Matrix<double, kN, kN> X = ::Eigen::Matrix<double, kN, kN>::Zero();
+
+ double ALFAR[kN * 2];
+ double ALFAI[kN * 2];
+ double BETA[kN * 2];
+ memset(ALFAR, 0, kN * 2);
+ memset(ALFAI, 0, kN * 2);
+ memset(BETA, 0, kN * 2);
+
+ int LDS = 2 * kN + kM;
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 *kN + kM> S_schur =
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 * kN + kM>::Zero();
+
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 *kN> T =
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 * kN>::Zero();
+ int LDT = 2 * kN + kM;
+
+ ::Eigen::Matrix<double, 2 * kN, 2 *kN> U =
+ ::Eigen::Matrix<double, 2 * kN, 2 * kN>::Zero();
+ int LDU = 2 * kN;
+
+ double TOL = 0.0;
+
+ int IWORK[2 * kN > kM ? 2 * kN : kM];
+ memset(IWORK, 0, 2 * kN > kM ? 2 * kN : kM);
+
+ int LDWORK = 16 * kN + 3 * kM + 16;
+
+ double DWORK[LDWORK];
+ memset(DWORK, 0, LDWORK);
+
+ int INFO = 0;
+
+ int BWORK[2 * kN];
+ memset(BWORK, 0, 2 * kN);
+
+ // TODO(austin): I can't tell if anything here is transposed...
+ sb02od_(&DICO, &JOBB, &FACT, &UPLO, &JOBL, &SORT, &N, &M, &P, A.data(),
+ &N, B.data(), &N, Q.data(), &N, R.data(), &M, nullptr, &LDL,
+ &RCOND, X.data(), &N, ALFAR, ALFAI, BETA, S_schur.data(), &LDS,
+ T.data(), &LDT, U.data(), &LDU, &TOL, IWORK, DWORK, &LDWORK, BWORK,
+ &INFO);
+ //::std::cout << "slycot result: " << INFO << ::std::endl;
+
+ *K = (R + B.transpose() * X * B).inverse() * B.transpose() * X * A;
+ if (S != nullptr) {
+ *S = X;
+ }
+}
+
+} // namespace controls
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DLQR_H_