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_