diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index dfdaef4..ae2ed51 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -4,10 +4,12 @@
         "trajectory.cc",
     ],
     hdrs = [
+        "dlqr.h",
         "trajectory.h",
     ],
     deps = [
         ":dynamics",
+        "//frc971/control_loops:jacobian",
         "//third_party/eigen",
     ],
 )
@@ -17,7 +19,11 @@
     srcs = [
         "trajectory_test.cc",
     ],
+    linkopts = ["-lslicot"],
+    restricted_to = ["//tools:k8"],
     deps = [
+        ":demo_path",
+        ":dynamics",
         ":trajectory",
         "//aos/testing:googletest",
         "//third_party/eigen",
@@ -38,6 +44,15 @@
     ],
 )
 
+cc_library(
+    name = "demo_path",
+    srcs = [
+        "demo_path.cc",
+    ],
+    hdrs = ["demo_path.h"],
+    deps = [":trajectory"],
+)
+
 cc_test(
     name = "dynamics_test",
     srcs = [
@@ -48,3 +63,19 @@
         "//aos/testing:googletest",
     ],
 )
+
+cc_binary(
+    name = "trajectory_plot",
+    srcs = [
+        "trajectory_plot.cc",
+    ],
+    linkopts = ["-lslicot"],
+    restricted_to = ["//tools:k8"],
+    deps = [
+        ":demo_path",
+        ":trajectory",
+        "//third_party/eigen",
+        "//third_party/matplotlib-cpp",
+    ],
+)
+
diff --git a/y2018/control_loops/superstructure/arm/demo_path.cc b/y2018/control_loops/superstructure/arm/demo_path.cc
new file mode 100644
index 0000000..6be0a06
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/demo_path.cc
@@ -0,0 +1,210 @@
+#include "y2018/control_loops/superstructure/arm/demo_path.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+::std::vector<::std::array<double, 6>> FlipPath(
+    ::std::initializer_list<::std::array<double, 6>> list) {
+  ::std::vector<::std::array<double, 6>> result;
+  result.reserve(list.size());
+  for (::std::array<double, 6> point : list) {
+    point[0] = M_PI_2 - point[0];
+    point[1] = M_PI_2 - point[1];
+    point[2] = -point[2];
+    point[3] = -point[3];
+    point[4] = -point[4];
+    point[5] = -point[5];
+    result.push_back(point);
+  }
+  return result;
+}
+
+
+Path MakeDemoPath() {
+  return Path(FlipPath(
+      {{{1.3583511559969876, 0.99753029519739866, 0.63708920330895369,
+         -0.77079007974101643, -0.21483375398380378, -0.17756921128311187}},
+       {{1.4037744780290744, 0.94141413786797179, 0.62102298265172207,
+         -0.78379235452915641, -0.23084099683729808, -0.18290283743090688}},
+       {{1.4401396868545593, 0.89467165253531666, 0.606891738771278,
+         -0.79478451004733031, -0.24685749210031543, -0.18849894149927623}},
+       {{1.4710837705397868, 0.85345617877170399, 0.59376006111610313,
+         -0.80464215016577489, -0.26311393698286972, -0.19415687958799988}},
+       {{1.4982967960378542, 0.81597755723156562, 0.58119389263733834,
+         -0.81376511301545618, -0.27984413029527577, -0.19986598102433009}},
+       {{1.5227269571172215, 0.78122893136122973, 0.56894801707769227,
+         -0.82237348805962973, -0.29704891442706877, -0.20550961547441812}},
+       {{1.5449685847030388, 0.74857685672553398, 0.55686625320351735,
+         -0.83060217676278458, -0.31482379304256025, -0.21106978462024978}},
+       {{1.5654226738527495, 0.71759174238125301, 0.54484048864732038,
+         -0.83853970802255351, -0.33321287621660045, -0.21650513557965106}},
+       {{1.5843743655705165, 0.68796601612512265, 0.53279106733541426,
+         -0.84624681894089859, -0.35226023809265267, -0.22178093106252236}},
+       {{1.6020345343012865, 0.65947020123078393, 0.52065635488289785,
+         -0.85376633812774205, -0.37188463022164508, -0.22678852570730737}},
+       {{1.6185639244576269, 0.63192742228662813, 0.50838674358661662,
+         -0.86112886314731996, -0.39227952043050979, -0.23159139751917215}},
+       {{1.6340880277208036, 0.60519768777296334, 0.49594100637973321,
+         -0.868356216187261, -0.41330592927121557, -0.23605025316897368}},
+       {{1.6487067026409843, 0.57916772325998889, 0.48328397846038601,
+         -0.87546364639743945, -0.43479093110952094, -0.24001893904573279}},
+       {{1.6625006419641934, 0.55374413183503834, 0.47038503527185527,
+         -0.88246128447218319, -0.45715591769925784, -0.2436817242766752}},
+       {{1.6755358637715485, 0.52884863988931285, 0.45721707379330323,
+         -0.88935513009814537, -0.48005035022840359, -0.24679412703629519}},
+       {{1.6878669166029825, 0.50441469954774187, 0.44375581873319114,
+         -0.89614774079971626, -0.50380737957408728, -0.24947659419788909}},
+       {{1.6995392207123023, 0.48038500205371104, 0.42997935653089725,
+         -0.90283871923908732, -0.52783372117172589, -0.25138272577381626}},
+       {{1.7105908129416818, 0.45670961972392227, 0.41586782149756141,
+         -0.90942506840468851, -0.55267961927927023, -0.25273346631273114}},
+       {{1.7210536699519405, 0.43334459201815007, 0.40140320353386438,
+         -0.91590145113584731, -0.57770009966870806, -0.25318354738409404}},
+       {{1.7309547270317198, 0.41025083198931545, 0.38656923494570777,
+         -0.92226038979969771, -0.60317645612010684, -0.25282447346857195}},
+       {{1.7403166729890138, 0.38739326814551689, 0.37135135004406289,
+         -0.92849242044318914, -0.62907036438628838, -0.25159784841893079}},
+       {{1.7491585775728877, 0.36474016215239319, 0.35573669259875446,
+         -0.93458622156486959, -0.6551663269414254, -0.24938020059543903}},
+       {{1.7574963917487576, 0.34226255982896742, 0.33971416817223166,
+         -0.94052872574050006, -0.68164592193873064, -0.24620769226541636}},
+       {{1.7653433501187576, 0.31993384454025714, 0.32327453032164366,
+         -0.94630522456833177, -0.70757451300020135, -0.24172061458903144}},
+       {{1.7727102970907476, 0.29772937021195395, 0.30641049133414705,
+         -0.95189947515500117, -0.73378453575677771, -0.23620138680600353}},
+       {{1.7796059529520747, 0.27562615695138953, 0.28911685939500692,
+         -0.95729381154041093, -0.75990897245769828, -0.22950448838605128}},
+       {{1.7860371320859008, 0.25360263640618674, 0.27139068895919466,
+         -0.9624692690918778, -0.78523056852177775, -0.22141489322699751}},
+       {{1.7920089227108962, 0.23163843702152076, 0.25323144050682866,
+         -0.96740572540110403, -0.81025448009679157, -0.21209581939563796}},
+       {{1.7975248354162883, 0.20971420159976159, 0.2346411508301583,
+         -0.97208205946674009, -0.83408589705170777, -0.20133249883457041}},
+       {{1.8025869261905516, 0.187811431247703, 0.21562459548465329,
+         -0.97647633551565383, -0.85752536967991499, -0.18935884543741083}},
+       {{1.8071958984562269, 0.16591235107247332, 0.19618945367704119,
+         -0.98056600913243175, -0.8793458743889202, -0.17593847769244828}},
+       {{1.8113511877225346, 0.14399979396679621, 0.17634645105214913,
+         -0.98432816133711831, -0.89983233183814704, -0.16120962647016274}},
+       {{1.8150510317784807, 0.12205709958524935, 0.15610948821796866,
+         -0.98773975706575867, -0.91852354093170219, -0.14517104001739675}},
+       {{1.8182925288197413, 0.10006802621144681, 0.13549574094137548,
+         -0.99077792879471627, -0.93602333307674279, -0.12800867856143744}},
+       {{1.821071685494891, 0.078016673692446373, 0.11452572935697461,
+         -0.99342028231522084, -0.95112385045254688, -0.10965064890920556}},
+       {{1.8233834565427998, 0.055887416001137141, 0.093223341590826445,
+         -0.99564522224667962, -0.96368362602021329, -0.090231797498176183}},
+       {{1.8252217774528923, 0.03366484230234558, 0.071615815013953282,
+         -0.99743229095507402, -0.97395276760288163, -0.069931055757802438}},
+       {{1.8265795913984428, 0.011333705660752967, 0.049733665943173709,
+         -0.99876251555204698, -0.98177893706111263, -0.048889055531294821}},
+       {{1.8274488715589303, -0.011121121248675203, 0.02761056315188902,
+         -0.99961875572762016, -0.98689335559342106, -0.027260179511112551}},
+       {{1.8278206398523622, -0.033714683874300592, 0.0052831412013899706,
+         -0.99998604411213976, -0.98885489268650051, -0.0052254487261787384}},
+       {{1.8276849830361703, -0.056461977241467148, -0.017209244112191453,
+         -0.99985190999321838, -0.98812791978941406, 0.017006330622021722}},
+       {{1.8270310671011307, -0.079377971247817633, -0.039824819694542858,
+         -0.99920667718760625, -0.98450390283226485, 0.039237693528591036}},
+       {{1.8258471508733223, -0.10247762977900209, -0.062519753866286318,
+         -0.99804372668560926, -0.97719363903338485, 0.061212532653782022}},
+       {{1.8241205997516765, -0.12577592348843702, -0.085248569812080663,
+         -0.99635971483445407, -0.9676884196650537, 0.08279433361404552}},
+       {{1.8218379005412455, -0.14928783595476772, -0.10796459505798382,
+         -0.99415473957224865, -0.95496997071328249, 0.103708042997466}},
+       {{1.8189846783932371, -0.17302836280361969, -0.13062045461685196,
+         -0.99143244693508337, -0.93935720014890423, 0.12375848445745616}},
+       {{1.8155457169306066, -0.19701250325490266, -0.15316858191907523,
+         -0.98820007362522466, -0.92129127811992539, 0.14279680293047756}},
+       {{1.8115049827211411, -0.22125524343262917, -0.17556174489393769,
+         -0.98446842190585071, -0.90059657633599033, 0.16060369204333666}},
+       {{1.8068456553568204, -0.24577153065112339, -0.19775357131740673,
+         -0.98025176614541798, -0.87754964758424281, 0.17703366424976713}},
+       {{1.8015501645066898, -0.27057623777063011, -0.21969906367786421,
+         -0.97556769186923664, -0.85256620853260556, 0.19199796793136439}},
+       {{1.795600235427889, -0.29568411659857541, -0.24135508622397667,
+         -0.97043687190554384, -0.82563764988942245, 0.20534143268497296}},
+       {{1.7889769445422168, -0.32110973920321939, -0.26268082100774776,
+         -0.96488278369690872, -0.79733324597202848, 0.2170659935022537}},
+       {{1.7816607868090406, -0.34686742590848585, -0.2836381738799823,
+         -0.95893137727265387, -0.76782873106500538, 0.22711183686998365}},
+       {{1.7736317567432636, -0.37297115865829411, -0.30419213280652752,
+         -0.95261070030659223, -0.73738784843863214, 0.23546528518662449}},
+       {{1.7648694450316516, -0.39943447838330554, -0.32431106313855179,
+         -0.94595049253433039, -0.70610753433104578, 0.2420821140887684}},
+       {{1.7553531527820141, -0.42627036498226889, -0.34396694344403017,
+         -0.93898175797923322, -0.67467492193224388, 0.24714550708779132}},
+       {{1.7450620254853675, -0.45349109855564618, -0.36313553561943973,
+         -0.93173632684916963, -0.64280482438985487, 0.25052642309609846}},
+       {{1.7339752087663065, -0.48110810061485765, -0.38179649614837224,
+         -0.92424641493966653, -0.61125522193929116, 0.2525024014306243}},
+       {{1.7220720279237476, -0.50913175415228962, -0.39993342129901005,
+         -0.91654419343972093, -0.58016017720123902, 0.25315181203742848}},
+       {{1.7093321931028456, -0.53757120171300954, -0.41753384214477446,
+         -0.90866137293483673, -0.54964898595174738, 0.25256545404213349}},
+       {{1.6957360316664474, -0.56643412097840984, -0.43458916639025197,
+         -0.9006288116955985, -0.51972307699036913, 0.25078642241782151}},
+       {{1.6812647489267576, -0.59572647787451594, -0.45109457862962754,
+         -0.89247615157546845, -0.49088885529812037, 0.24811508685641395}},
+       {{1.6659007178300929, -0.62545225787260295, -0.46704890024189227,
+         -0.88423148823305242, -0.46296888429931843, 0.24453849915066983}},
+       {{1.6496277974361373, -0.65561317697342414, -0.48245442463761296,
+         -0.87592107415428122, -0.43630053035835431, 0.24031239163872659}},
+       {{1.6324316790779632, -0.68620837486997288, -0.49731672574028513,
+         -0.86756906024763358, -0.41095471910587472, 0.23557116495526012}},
+       {{1.6143002579172752, -0.7172340939700278, -0.51164445121259294,
+         -0.85919727393850864, -0.38675403181117768, 0.23030820687574571}},
+       {{1.5952240262184549, -0.748683349319481, -0.52544910775441078,
+         -0.85082503204836046, -0.36408233991880601, 0.22484810685703166}},
+       {{1.5751964830679697, -0.780545595975073, -0.53874483689504815,
+         -0.84246899095392713, -0.34276938388236106, 0.21919491474821423}},
+       {{1.5542145534957212, -0.81280640198469889, -0.55154819264623711,
+         -0.83414302801658013, -0.32291038254364612, 0.21351295685970414}},
+       {{1.5322790080695103, -0.84544713677503402, -0.56387791937564813,
+         -0.82585815491559456, -0.30444498192901781, 0.20786805829508584}},
+       {{1.509394872119298, -0.87844468632349793, -0.57575473429345714,
+         -0.81762245929198296, -0.28737482236853346, 0.20236376870812761}},
+       {{1.4855718119204786, -0.91177120788178356, -0.58720111439137757,
+         -0.8094410733694728, -0.27173204177162957, 0.1971250636942595}},
+       {{1.4608244835702884, -0.94539393807497152, -0.59824108887263983,
+         -0.80131616705547515, -0.25741396251510884, 0.19217806498346204}},
+       {{1.4351728290976418, -0.97927506876108006, -0.60890003734400222,
+         -0.79324696313473042, -0.24447483076597848, 0.18765975242966434}},
+       {{1.4086423037364302, -1.0133717049339268, -0.61920449072176709,
+         -0.78522977444184905, -0.2328103488745005, 0.18358578028961331}},
+       {{1.3812640184459468, -1.047635918033063, -0.62918193604047223,
+         -0.77725805969469564, -0.22240924347971955, 0.18003762666126619}},
+       {{1.3530747828369902, -1.0820149061676485, -0.63886062143319422,
+         -0.76932249829443622, -0.21321450222713362, 0.17705731568712871}},
+       {{1.324117035769417, -1.1164512699023377, -0.64826936036845306,
+         -0.76141108240389876, -0.20523063810290823, 0.17473421668463424}},
+       {{1.294438654066159, -1.1508834084076087, -0.6574373330314689,
+         -0.75350922564788103, -0.19832793065660315, 0.17304104866571926}},
+       {{1.2640926339871807, -1.1852460360561055, -0.66639388399919663,
+         -0.74559988691553936, -0.1925723524639939, 0.17211494490833781}},
+       {{1.2331366451651338, -1.2194708141674491, -0.67516831336894412,
+         -0.73766370970960415, -0.18786875722059337, 0.17195217715824379}},
+       {{1.2016324623545263, -1.2534870868845998, -0.68378966242702388,
+         -0.72967917440333774, -0.18419866624510314, 0.17261421082264014}},
+       {{1.1696452862242701, -1.2872227045046678, -0.6922864921531301,
+         -0.72162276348679166, -0.18150423850780073, 0.17412530594411615}},
+       {{1.1372429700984137, -1.3206049124326309, -0.70068665547523046,
+         -0.71346913797229905, -0.17977253485932801, 0.17655152952198189}},
+       {{1.104495174566031, -1.3535612797241663, -0.70901706362016093,
+         -0.70519132403585671, -0.17898758721428376, 0.17995840096834861}},
+       {{1.0714724758096574, -1.3860206383273435, -0.71730344800943324,
+         -0.69676090839955884, -0.17907637813730631, 0.18435585053172451}},
+       {{1.0382454559924987, -1.4179140029119093, -0.72557011960647111,
+         -0.68814824095848082, -0.18003314612431659, 0.18982321746977993}},
+       {{1.0048838048727911, -1.4491754417344611, -0.73383972725116353,
+         -0.67932264404179699, -0.18184420373071258, 0.19643734513631667}},
+       {{0.97145546090878643, -1.4797428713062153, -0.74213301743428883,
+         -0.67025262732336799, -0.18446052990619061, 0.20424250843873848}}}));
+}
+
+}  // namespace arm
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/demo_path.h b/y2018/control_loops/superstructure/arm/demo_path.h
new file mode 100644
index 0000000..ccf4276
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/demo_path.h
@@ -0,0 +1,18 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
+
+#include "y2018/control_loops/superstructure/arm/trajectory.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+Path MakeDemoPath();
+
+}  // namespace arm
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2018
+
+#endif  // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
diff --git a/y2018/control_loops/superstructure/arm/dlqr.h b/y2018/control_loops/superstructure/arm/dlqr.h
new file mode 100644
index 0000000..d33ee2b
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/dlqr.h
@@ -0,0 +1,98 @@
+#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_
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/y2018/control_loops/superstructure/arm/trajectory.cc
index fb1e0a5..fbe38eb 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory.cc
@@ -1,5 +1,10 @@
 #include "y2018/control_loops/superstructure/arm/trajectory.h"
 
+#include "Eigen/Dense"
+#include "frc971/control_loops/jacobian.h"
+#include "y2018/control_loops/superstructure/arm/dlqr.h"
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
+
 namespace y2018 {
 namespace control_loops {
 namespace superstructure {
@@ -245,7 +250,7 @@
     // Subdivide the plan step size into kNumSteps steps.
     double integrated_distance = 0.0;
     double integrated_velocity = previous_velocity;
-    constexpr int kNumSteps = 10;
+    constexpr int kNumSteps = 50;
     for (int j = 0; j < kNumSteps; ++j) {
       // Compute the acceleration with respect to time.
       const double acceleration = FeasableBackwardsAcceleration(
@@ -303,6 +308,191 @@
   return max_dvelocity_forward_pass;
 }
 
+void TrajectoryFollower::Reset() {
+  next_goal_ = last_goal_ = goal_ = ::Eigen::Matrix<double, 2, 1>::Zero();
+  U_ = ::Eigen::Matrix<double, 2, 1>::Zero();
+  goal_acceleration_ = 0.0;
+}
+
+::Eigen::Matrix<double, 2, 4> TrajectoryFollower::K_at_state(
+    const ::Eigen::Matrix<double, 4, 1> &X,
+    const ::Eigen::Matrix<double, 2, 1> &U) {
+  constexpr double q_pos = 0.2;
+  constexpr double q_vel = 4.0;
+  const ::Eigen::DiagonalMatrix<double, 4> Q =
+      (::Eigen::DiagonalMatrix<double, 4>().diagonal()
+           << 1.0 / ::std::pow(q_pos, 2),
+       1.0 / ::std::pow(q_vel, 2), 1.0 / ::std::pow(q_pos, 2),
+       1.0 / ::std::pow(q_vel, 2))
+          .finished()
+          .asDiagonal();
+
+  const ::Eigen::DiagonalMatrix<double, 2> R =
+      (::Eigen::DiagonalMatrix<double, 2>().diagonal()
+           << 1.0 / ::std::pow(12.0, 2),
+       1.0 / ::std::pow(12.0, 2))
+          .finished()
+          .asDiagonal();
+
+  const ::Eigen::Matrix<double, 4, 4> final_A =
+      ::frc971::control_loops::NumericalJacobianX(
+          Dynamics::UnboundedDiscreteDynamics, X, U, 0.00505);
+  const ::Eigen::Matrix<double, 4, 2> final_B =
+      ::frc971::control_loops::NumericalJacobianU(
+          Dynamics::UnboundedDiscreteDynamics, X, U, 0.00505);
+
+  ::Eigen::Matrix<double, 2, 4> K;
+  ::Eigen::Matrix<double, 4, 4> S;
+  ::frc971::controls::dlqr<4, 2>(final_A, final_B, Q, R, &K, &S);
+  return K;
+}
+
+void TrajectoryFollower::USaturationSearch(
+    double goal_distance, double last_goal_distance, double goal_velocity,
+    double last_goal_velocity, double fraction_along_path,
+    const ::Eigen::Matrix<double, 2, 4> &K,
+    const ::Eigen::Matrix<double, 4, 1> &X, const Trajectory &trajectory,
+    ::Eigen::Matrix<double, 2, 1> *U, double *saturation_goal_velocity,
+    double *saturation_goal_acceleration) {
+  double saturation_goal_distance =
+      ((goal_distance - last_goal_distance) * fraction_along_path +
+       last_goal_distance);
+
+  const ::Eigen::Matrix<double, 2, 1> theta_t =
+      trajectory.ThetaT(saturation_goal_distance);
+  *saturation_goal_velocity = trajectory.InterpolateVelocity(
+      saturation_goal_distance, last_goal_distance, goal_distance,
+      last_goal_velocity, goal_velocity);
+  *saturation_goal_acceleration = trajectory.InterpolateAcceleration(
+      last_goal_distance, goal_distance, last_goal_velocity, goal_velocity);
+  const ::Eigen::Matrix<double, 2, 1> omega_t =
+      trajectory.OmegaT(saturation_goal_distance, *saturation_goal_velocity);
+  const ::Eigen::Matrix<double, 2, 1> alpha_t =
+      trajectory.AlphaT(saturation_goal_distance, *saturation_goal_velocity,
+                        *saturation_goal_acceleration);
+  const ::Eigen::Matrix<double, 4, 1> R = trajectory.R(theta_t, omega_t);
+  const ::Eigen::Matrix<double, 2, 1> U_ff =
+      Dynamics::FF_U(R, omega_t, alpha_t);
+
+  *U = U_ff + K * (R - X);
+}
+
+::Eigen::Matrix<double, 2, 1> TrajectoryFollower::PlanNextGoal(
+    const ::Eigen::Matrix<double, 2, 1> &goal, double vmax, double dt) {
+  // Figure out where we would be if we were to accelerate as fast as
+  // our constraints allow.
+  ::Eigen::Matrix<double, 2, 1> next_goal = ::frc971::control_loops::RungeKutta(
+      [this, &vmax](const ::Eigen::Matrix<double, 2, 1> &X) {
+        return (::Eigen::Matrix<double, 2, 1>() << X(1),
+                trajectory_->FeasableForwardsAcceleration(X(0), X(1), vmax,
+                                                          alpha_unitizer_))
+            .finished();
+      },
+      goal, dt);
+
+  // Min that with the backwards pass velocity in case the backwards pass
+  // wants us to slow down.
+  const double next_trajectory_velocity =
+      trajectory_->GetDVelocity(next_goal(0));
+  if (next_trajectory_velocity < next_goal(1)) {
+    next_goal(1) = next_trajectory_velocity;
+    // And then recompute how far to go with our new trajectory in mind.
+    double goal_acceleration = trajectory_->InterpolateAcceleration(
+        goal(0), next_goal(0), goal(1), next_goal(1));
+    next_goal(0) = (goal(0) + goal(1) * dt + 0.5 * dt * dt * goal_acceleration);
+    next_goal(1) = trajectory_->GetDVelocity(next_goal(0));
+  }
+  return next_goal;
+}
+
+void TrajectoryFollower::Update(const ::Eigen::Matrix<double, 4, 1> &X,
+                                double dt, double vmax) {
+  // To avoid exposing the new goals before the outer code has a chance to
+  // querry the internal state, move to the new goals here.
+  last_goal_ = goal_;
+  goal_ = next_goal_;
+
+  if (::std::abs(goal_(0) - path_->length()) < 1e-2) {
+    // If we go backwards along the path near the goal, snap us to the end
+    // point or we'll never actually finish.
+    if (goal_acceleration_ * dt + goal_(1) < 0.0 ||
+        goal_(0) > path_->length()) {
+      goal_(0) = path_->length();
+      goal_(1) = 0.0;
+    }
+  }
+
+  if (goal_(0) == path_->length()) {
+    next_goal_(0) = goal_(0);
+    next_goal_(1) = 0.0;
+    goal_acceleration_ = 0.0;
+  } else {
+    // Figure out where we would be if we were to accelerate as fast as
+    // our constraints allow.
+    next_goal_ = PlanNextGoal(goal_, vmax, dt);
+
+    goal_acceleration_ = trajectory_->InterpolateAcceleration(
+        goal_(0), next_goal_(0), goal_(1), next_goal_(1));
+  }
+
+  const ::Eigen::Matrix<double, 2, 1> theta_t = trajectory_->ThetaT(goal_(0));
+  const ::Eigen::Matrix<double, 2, 1> omega_t =
+      trajectory_->OmegaT(goal_(0), goal_(1));
+  const ::Eigen::Matrix<double, 2, 1> alpha_t =
+      trajectory_->AlphaT(goal_(0), goal_(1), goal_acceleration_);
+
+  const ::Eigen::Matrix<double, 4, 1> R = trajectory_->R(theta_t, omega_t);
+
+  U_ff_ = Dynamics::FF_U(R, omega_t, alpha_t);
+
+  const ::Eigen::Matrix<double, 2, 4> K = K_at_state(X, U_ff_);
+  U_ = U_unsaturated_ = U_ff_ + K * (R - X);
+
+  // Ok, now we know if we are staturated or not.  If we are, time to search
+  // between here and our previous goal either until we find a state where we
+  // aren't saturated, or we are really close to our starting point.
+  fraction_along_path_ = 1.0;
+  if ((U_.array().abs() > 12.0).any()) {
+    // Saturated.  Let's do a binary search.
+    double step_size;
+    if ((goal_(0) - last_goal_(0)) < 1e-8) {
+      // print "Not bothering to move"
+      // Avoid the divide by 0 when interpolating.  Just don't move since we
+      // are saturated.
+      fraction_along_path_ = 0.0;
+      step_size = 0.0;
+    } else {
+      fraction_along_path_ = 0.5;
+      step_size = 0.5;
+    }
+
+    // Pull us back to the previous point until we aren't saturated anymore.
+    double saturation_goal_velocity;
+    double saturation_goal_acceleration;
+    while (step_size > 0.01) {
+      USaturationSearch(goal_(0), last_goal_(0), goal_(1), last_goal_(1),
+                        fraction_along_path_, K, X, *trajectory_, &U_,
+                        &saturation_goal_velocity,
+                        &saturation_goal_acceleration);
+      step_size = step_size * 0.5;
+      if ((U_.array().abs() > 12.0).any()) {
+        fraction_along_path_ -= step_size;
+      } else {
+        fraction_along_path_ += step_size;
+      }
+    }
+
+    goal_(0) =
+        ((goal_(0) - last_goal_(0)) * fraction_along_path_ + last_goal_(0));
+    goal_(1) = saturation_goal_velocity;
+
+    next_goal_ = PlanNextGoal(goal_, vmax, dt);
+
+    goal_acceleration_ = trajectory_->InterpolateAcceleration(
+        goal_(0), next_goal_(0), goal_(1), next_goal_(1));
+  }
+}
+
 }  // namespace arm
 }  // namespace superstructure
 }  // namespace control_loops
diff --git a/y2018/control_loops/superstructure/arm/trajectory.h b/y2018/control_loops/superstructure/arm/trajectory.h
index 2c9d68c..5ecc783 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.h
+++ b/y2018/control_loops/superstructure/arm/trajectory.h
@@ -3,8 +3,6 @@
 
 #include "Eigen/Dense"
 
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
 namespace y2018 {
 namespace control_loops {
 namespace superstructure {
@@ -168,6 +166,87 @@
     return max_dvelocity_forward_pass_;
   }
 
+  // Returns the interpolated velocity at the distance d along the path.  The
+  // math assumes constant acceleration from point (d0, v0), to point (d1, v1),
+  // and that we are at point d between the two.
+  static double InterpolateVelocity(double d, double d0, double d1, double v0,
+                                    double v1) {
+    // We don't support negative velocities.
+    if (v0 < 0 || v1 < 0) {
+      abort();
+    }
+    if (d <= d0) {
+      return v0;
+    }
+    if (d >= d1) {
+      return v1;
+    }
+    return ::std::sqrt(v0 * v0 + (v1 * v1 - v0 * v0) * (d - d0) / (d1 - d0));
+  }
+
+  // Computes the path distance velocity of the plan as a function of the
+  // distance.
+  double GetDVelocity(double d) {
+    ::std::pair<size_t, size_t> indices = IndicesForDistance(d);
+    const double v0 = max_dvelocity_[indices.first];
+    const double v1 = max_dvelocity_[indices.second];
+    const double d0 = DistanceForIndex(indices.first);
+    const double d1 = DistanceForIndex(indices.second);
+    return InterpolateVelocity(d, d0, d1, v0, v1);
+  }
+
+  // Computes the path distance acceleration of the plan as a function of the
+  // distance.
+  double GetDAcceleration(double distance) const {
+    ::std::pair<size_t, size_t> indices = IndicesForDistance(distance);
+    const double v0 = max_dvelocity_[indices.first];
+    const double v1 = max_dvelocity_[indices.second];
+    const double d0 = DistanceForIndex(indices.first);
+    const double d1 = DistanceForIndex(indices.second);
+    return InterpolateAcceleration(d0, d1, v0, v1);
+  }
+
+  // Returns the acceleration along the path segment assuming constant
+  // acceleration.
+  static double InterpolateAcceleration(double d0, double d1, double v0,
+                                        double v1) {
+    return 0.5 * (::std::pow(v1, 2) - ::std::pow(v0, 2)) / (d1 - d0);
+  }
+
+  ::Eigen::Matrix<double, 2, 1> ThetaT(double d) const {
+    return path_->Theta(d);
+  }
+
+  // Returns d theta/dt at a specified path distance and velocity.
+  ::Eigen::Matrix<double, 2, 1> OmegaT(double distance, double velocity) const {
+    if (distance > path_->length() || distance < 0.0) {
+      return ::Eigen::Matrix<double, 2, 1>::Zero();
+    } else {
+      return path_->Omega(distance) * velocity;
+    }
+  }
+
+  // Returns d^2 theta/dt^2 at a specified path distance, velocity and
+  // acceleration.
+  ::Eigen::Matrix<double, 2, 1> AlphaT(double distance, double velocity,
+                                       double acceleration) const {
+    if (distance > path_->length() || distance < 0.0) {
+      return ::Eigen::Matrix<double, 2, 1>::Zero();
+    } else {
+      return path_->Alpha(distance) * ::std::pow(velocity, 2) +
+             path_->Omega(distance) * acceleration;
+    }
+  }
+
+  // Converts a theta and omega vector to a full state vector.
+  static ::Eigen::Matrix<double, 4, 1> R(
+      ::Eigen::Matrix<double, 2, 1> theta_t,
+      ::Eigen::Matrix<double, 2, 1> omega_t) {
+    return (::Eigen::Matrix<double, 4, 1>() << theta_t(0, 0), omega_t(0, 0),
+            theta_t(1, 0), omega_t(1, 0))
+        .finished();
+  }
+
  private:
   friend class testing::TrajectoryTest_IndicesForDistanceTest_Test;
 
@@ -202,6 +281,82 @@
   ::std::vector<double> max_dvelocity_forward_pass_;
 };
 
+// This class tracks the current goal along trajectories and paths.
+class TrajectoryFollower {
+ public:
+  TrajectoryFollower(Path *const path, Trajectory *const trajectory,
+                     const ::Eigen::Matrix<double, 2, 2> alpha_unitizer)
+      : path_(path), trajectory_(trajectory), alpha_unitizer_(alpha_unitizer) {
+    Reset();
+  }
+
+  // Returns the goal distance along the path.
+  const ::Eigen::Matrix<double, 2, 1> &goal() const { return goal_; }
+  double goal(int i) const { return goal_(i); }
+
+  // Starts over at the beginning of the path.
+  // TODO(austin): Allow switching the path.
+  void Reset();
+
+  // Returns the controller gain at the provided state.
+  ::Eigen::Matrix<double, 2, 4> K_at_state(
+      const ::Eigen::Matrix<double, 4, 1> &X,
+      const ::Eigen::Matrix<double, 2, 1> &U);
+
+  // Returns the voltage, velocity and acceleration if we were to be partially
+  // along the path.
+  void USaturationSearch(double goal_distance, double last_goal_distance,
+                         double goal_velocity, double last_goal_velocity,
+                         double fraction_along_path,
+                         const ::Eigen::Matrix<double, 2, 4> &K,
+                         const ::Eigen::Matrix<double, 4, 1> &X,
+                         const Trajectory &trajectory,
+                         ::Eigen::Matrix<double, 2, 1> *U,
+                         double *saturation_goal_velocity,
+                         double *saturation_goal_acceleration);
+
+  // Returns the next goal given a planning vmax and timestep.  This ignores the
+  // backwards pass.
+  ::Eigen::Matrix<double, 2, 1> PlanNextGoal(
+      const ::Eigen::Matrix<double, 2, 1> &goal, double vmax, double dt);
+
+  // Plans the next cycle and updates the internal state for consumption.
+  void Update(const ::Eigen::Matrix<double, 4, 1> &X, double dt, double vmax);
+
+  // Returns the goal acceleration for this cycle.
+  double goal_acceleration() const { return goal_acceleration_; }
+
+  // Returns U(s) for this cycle.
+  const ::Eigen::Matrix<double, 2, 1> &U() const { return U_; }
+  const ::Eigen::Matrix<double, 2, 1> &U_unsaturated() const {
+    return U_unsaturated_;
+  }
+  const ::Eigen::Matrix<double, 2, 1> &U_ff() const { return U_ff_; }
+
+  double fraction_along_path() const { return fraction_along_path_; }
+
+ private:
+  // The path to follow.
+  Path *const path_ = nullptr;
+  // The trajectory plan.
+  Trajectory *const trajectory_ = nullptr;
+  const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
+
+  // The current goal.
+  ::Eigen::Matrix<double, 2, 1> goal_;
+  // The previously executed goal.
+  ::Eigen::Matrix<double, 2, 1> last_goal_;
+  // The goal to use next cycle.  We always plan 1 cycle ahead.
+  ::Eigen::Matrix<double, 2, 1> next_goal_;
+
+  ::Eigen::Matrix<double, 2, 1> U_;
+  ::Eigen::Matrix<double, 2, 1> U_unsaturated_;
+  ::Eigen::Matrix<double, 2, 1> U_ff_;
+  double goal_acceleration_ = 0.0;
+
+  double fraction_along_path_ = 1.0;
+};
+
 }  // namespace arm
 }  // namespace superstructure
 }  // namespace control_loops
diff --git a/y2018/control_loops/superstructure/arm/trajectory_plot.cc b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
new file mode 100644
index 0000000..0b808f8
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
@@ -0,0 +1,232 @@
+#include "y2018/control_loops/superstructure/arm/trajectory.h"
+
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#include "y2018/control_loops/superstructure/arm/demo_path.h"
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+void Main() {
+  Path p = MakeDemoPath();
+  Trajectory trajectory(&p, 0.001);
+
+  constexpr double kAlpha0Max = 40.0;
+  constexpr double kAlpha1Max = 60.0;
+  constexpr double vmax = 11.95;
+  constexpr double sim_dt = 0.00505;
+
+  const ::Eigen::Matrix<double, 2, 2> alpha_unitizer =
+      (::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max, 0.0, 0.0,
+       1.0 / kAlpha1Max)
+          .finished();
+  trajectory.OptimizeTrajectory(alpha_unitizer, vmax);
+
+  ::std::vector<double> distance_array = trajectory.DistanceArray();
+
+  ::std::vector<double> theta0_array;
+  ::std::vector<double> theta1_array;
+  ::std::vector<double> omega0_array;
+  ::std::vector<double> omega1_array;
+  ::std::vector<double> alpha0_array;
+  ::std::vector<double> alpha1_array;
+
+  for (const double d : distance_array) {
+    const ::Eigen::Matrix<double, 2, 1> theta = p.Theta(d);
+    const ::Eigen::Matrix<double, 2, 1> omega = p.Omega(d);
+    const ::Eigen::Matrix<double, 2, 1> alpha = p.Alpha(d);
+    theta0_array.push_back(theta(0, 0));
+    theta1_array.push_back(theta(1, 0));
+    omega0_array.push_back(omega(0, 0));
+    omega1_array.push_back(omega(1, 0));
+    alpha0_array.push_back(alpha(0, 0));
+    alpha1_array.push_back(alpha(1, 0));
+  }
+
+  // Next step: see what U is as a function of distance.
+  ::std::vector<double> Uff0_distance_array;
+  ::std::vector<double> Uff1_distance_array;
+
+  for (const double distance : distance_array) {
+    const double goal_velocity = trajectory.GetDVelocity(distance);
+    const double goal_acceleration = trajectory.GetDAcceleration(distance);
+    const ::Eigen::Matrix<double, 2, 1> theta_t = trajectory.ThetaT(distance);
+    const ::Eigen::Matrix<double, 2, 1> omega_t =
+        trajectory.OmegaT(distance, goal_velocity);
+    const ::Eigen::Matrix<double, 2, 1> alpha_t =
+        trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+    const ::Eigen::Matrix<double, 4, 1> R = trajectory.R(theta_t, omega_t);
+    const ::Eigen::Matrix<double, 2, 1> U = Dynamics::FF_U(R, omega_t, alpha_t);
+
+    Uff0_distance_array.push_back(U(0));
+    Uff1_distance_array.push_back(U(1));
+  }
+
+  double t = 0;
+  ::Eigen::Matrix<double, 4, 1> X;
+  {
+    ::Eigen::Matrix<double, 2, 1> theta_t = trajectory.ThetaT(0.0);
+    X << theta_t(0), 0.0, theta_t(1), 0.0;
+  }
+
+  TrajectoryFollower follower(&p, &trajectory, alpha_unitizer);
+
+  ::std::vector<double> t_array;
+  ::std::vector<double> theta0_goal_t_array;
+  ::std::vector<double> theta1_goal_t_array;
+  ::std::vector<double> omega0_goal_t_array;
+  ::std::vector<double> omega1_goal_t_array;
+  ::std::vector<double> alpha0_goal_t_array;
+  ::std::vector<double> alpha1_goal_t_array;
+  ::std::vector<double> theta0_t_array;
+  ::std::vector<double> omega0_t_array;
+  ::std::vector<double> theta1_t_array;
+  ::std::vector<double> omega1_t_array;
+  ::std::vector<double> distance_t_array;
+  ::std::vector<double> velocity_t_array;
+  ::std::vector<double> acceleration_t_array;
+  ::std::vector<double> u0_unsaturated_array;
+  ::std::vector<double> u1_unsaturated_array;
+  ::std::vector<double> alpha0_t_array;
+  ::std::vector<double> alpha1_t_array;
+  ::std::vector<double> uff0_array;
+  ::std::vector<double> uff1_array;
+  ::std::vector<double> u0_array;
+  ::std::vector<double> u1_array;
+
+  while (t < 1.0) {
+    t_array.push_back(t);
+    follower.Update(X, sim_dt, vmax);
+
+    const ::Eigen::Matrix<double, 2, 1> theta_t =
+        trajectory.ThetaT(follower.goal()(0));
+    const ::Eigen::Matrix<double, 2, 1> omega_t =
+        trajectory.OmegaT(follower.goal()(0), follower.goal()(1));
+    const ::Eigen::Matrix<double, 2, 1> alpha_t = trajectory.AlphaT(
+        follower.goal()(0), follower.goal()(1), follower.goal_acceleration());
+
+    theta0_goal_t_array.push_back(theta_t(0));
+    theta1_goal_t_array.push_back(theta_t(1));
+    omega0_goal_t_array.push_back(omega_t(0));
+    omega1_goal_t_array.push_back(omega_t(1));
+    alpha0_goal_t_array.push_back(alpha_t(0));
+    alpha1_goal_t_array.push_back(alpha_t(1));
+    theta0_t_array.push_back(X(0));
+    omega0_t_array.push_back(X(1));
+    theta1_t_array.push_back(X(2));
+    omega1_t_array.push_back(X(3));
+
+    distance_t_array.push_back(follower.goal()(0));
+    velocity_t_array.push_back(follower.goal()(1));
+    acceleration_t_array.push_back(follower.goal_acceleration());
+
+    u0_unsaturated_array.push_back(follower.U_unsaturated()(0));
+    u1_unsaturated_array.push_back(follower.U_unsaturated()(1));
+
+    const ::Eigen::Matrix<double, 4, 1> xdot =
+        Dynamics::Acceleration(X, follower.U());
+
+    X = Dynamics::UnboundedDiscreteDynamics(X, follower.U(), sim_dt);
+
+    alpha0_t_array.push_back(xdot(1));
+    alpha1_t_array.push_back(xdot(3));
+
+    uff0_array.push_back(follower.U_ff()(0));
+    uff1_array.push_back(follower.U_ff()(1));
+    u0_array.push_back(follower.U()(0));
+    u1_array.push_back(follower.U()(1));
+
+    t += sim_dt;
+  }
+
+  matplotlibcpp::figure();
+  matplotlibcpp::title("Trajectory");
+  matplotlibcpp::plot(theta0_array, theta1_array, {{"label", "desired path"}});
+  matplotlibcpp::plot(theta0_t_array, theta1_t_array,
+                      {{"label", "actual path"}});
+  matplotlibcpp::legend();
+
+  matplotlibcpp::figure();
+  matplotlibcpp::plot(distance_array, theta0_array, {{"label", "theta0"}});
+  matplotlibcpp::plot(distance_array, theta1_array, {{"label", "theta1"}});
+  matplotlibcpp::plot(distance_array, omega0_array, {{"label", "omega0"}});
+  matplotlibcpp::plot(distance_array, omega1_array, {{"label", "omega1"}});
+  matplotlibcpp::plot(distance_array, alpha0_array, {{"label", "alpha0"}});
+  matplotlibcpp::plot(distance_array, alpha1_array, {{"label", "alpha1"}});
+  matplotlibcpp::legend();
+
+  matplotlibcpp::figure();
+  matplotlibcpp::plot(distance_array, trajectory.max_dvelocity_unfiltered(),
+                      {{"label", "pass0"}});
+  matplotlibcpp::plot(distance_array, trajectory.max_dvelocity(),
+                      {{"label", "passb"}});
+  matplotlibcpp::plot(distance_array, trajectory.max_dvelocity_forward_pass(),
+                      {{"label", "passf"}});
+  matplotlibcpp::legend();
+
+  matplotlibcpp::figure();
+  matplotlibcpp::plot(t_array, alpha0_goal_t_array,
+                      {{"label", "alpha0_t_goal"}});
+  matplotlibcpp::plot(t_array, alpha0_t_array, {{"label", "alpha0_t"}});
+  matplotlibcpp::plot(t_array, alpha1_goal_t_array,
+                      {{"label", "alpha1_t_goal"}});
+  matplotlibcpp::plot(t_array, alpha1_t_array, {{"label", "alpha1_t"}});
+  matplotlibcpp::plot(t_array, distance_t_array, {{"label", "distance_t"}});
+  matplotlibcpp::plot(t_array, velocity_t_array, {{"label", "velocity_t"}});
+  matplotlibcpp::plot(t_array, acceleration_t_array,
+                      {{"label", "acceleration_t"}});
+  matplotlibcpp::legend();
+
+  matplotlibcpp::figure();
+  matplotlibcpp::title("Angular Velocities");
+  matplotlibcpp::plot(t_array, omega0_goal_t_array,
+                      {{"label", "omega0_t_goal"}});
+  matplotlibcpp::plot(t_array, omega0_t_array, {{"label", "omega0_t"}});
+  matplotlibcpp::plot(t_array, omega1_goal_t_array,
+                      {{"label", "omega1_t_goal"}});
+  matplotlibcpp::plot(t_array, omega1_t_array, {{"label", "omega1_t"}});
+  matplotlibcpp::legend();
+
+  matplotlibcpp::figure();
+  matplotlibcpp::title("Voltages");
+  matplotlibcpp::plot(t_array, u0_unsaturated_array, {{"label", "u0_full"}});
+  matplotlibcpp::plot(t_array, u0_array, {{"label", "u0"}});
+  matplotlibcpp::plot(t_array, uff0_array, {{"label", "uff0"}});
+  matplotlibcpp::plot(t_array, u1_unsaturated_array, {{"label", "u1_full"}});
+  matplotlibcpp::plot(t_array, u1_array, {{"label", "u1"}});
+  matplotlibcpp::plot(t_array, uff1_array, {{"label", "uff1"}});
+  matplotlibcpp::legend();
+
+  matplotlibcpp::figure();
+  matplotlibcpp::title("Angles");
+  matplotlibcpp::plot(t_array, theta0_goal_t_array,
+                      {{"label", "theta0_t_goal"}});
+  matplotlibcpp::plot(t_array, theta0_t_array, {{"label", "theta0_t"}});
+  matplotlibcpp::plot(t_array, theta1_goal_t_array,
+                      {{"label", "theta1_t_goal"}});
+  matplotlibcpp::plot(t_array, theta1_t_array, {{"label", "theta1_t"}});
+  matplotlibcpp::legend();
+
+/*
+  matplotlibcpp::figure();
+  matplotlibcpp::title("ff for distance");
+  matplotlibcpp::plot(distance_array, Uff0_distance_array, {{"label", "ff0"}});
+  matplotlibcpp::plot(distance_array, Uff1_distance_array, {{"label", "ff1"}});
+  matplotlibcpp::legend();
+*/
+
+  matplotlibcpp::show();
+}
+
+}  // namespace arm
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2018
+
+int main(int /*argc*/, const char * /*argv*/ []) {
+  ::y2018::control_loops::superstructure::arm::Main();
+  return 0;
+}
diff --git a/y2018/control_loops/superstructure/arm/trajectory_test.cc b/y2018/control_loops/superstructure/arm/trajectory_test.cc
index a4e7822..3c5d6a1 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_test.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_test.cc
@@ -1,6 +1,8 @@
 #include "y2018/control_loops/superstructure/arm/trajectory.h"
 
 #include "gtest/gtest.h"
+#include "y2018/control_loops/superstructure/arm/demo_path.h"
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
 
 namespace y2018 {
 namespace control_loops {
@@ -113,6 +115,69 @@
   EXPECT_EQ(3.0, t.DistanceForIndex(30));
 }
 
+// Tests that we can correctly interpolate velocities between two points
+TEST(TrajectoryTest, InterpolateVelocity) {
+  // x = 0.5 * a * t^2
+  // v = a * t
+  // a = 2.0
+  EXPECT_EQ(0.0, Trajectory::InterpolateVelocity(0.0, 0.0, 1.0, 0.0, 2.0));
+  EXPECT_EQ(2.0, Trajectory::InterpolateVelocity(1.0, 0.0, 1.0, 0.0, 2.0));
+  EXPECT_EQ(0.0, Trajectory::InterpolateVelocity(-1.0, 0.0, 1.0, 0.0, 2.0));
+  EXPECT_EQ(2.0, Trajectory::InterpolateVelocity(20.0, 0.0, 1.0, 0.0, 2.0));
+}
+
+// Tests that we can correctly interpolate velocities between two points
+TEST(TrajectoryTest, InterpolateAcceleration) {
+  // x = 0.5 * a * t^2
+  // v = a * t
+  // a = 2.0
+  EXPECT_EQ(2.0, Trajectory::InterpolateAcceleration(0.0, 1.0, 0.0, 2.0));
+}
+
+// Tests that we can follow a path.  Look at :trajectory_plot if you want to see
+// the path.
+TEST(TrajectoryTest, RunTrajectory) {
+  Path path = MakeDemoPath();
+  Trajectory trajectory(&path, 0.001);
+
+  constexpr double kAlpha0Max = 40.0;
+  constexpr double kAlpha1Max = 60.0;
+  constexpr double vmax = 11.95;
+
+  const ::Eigen::Matrix<double, 2, 2> alpha_unitizer =
+      (::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max, 0.0, 0.0,
+       1.0 / kAlpha1Max)
+          .finished();
+  trajectory.OptimizeTrajectory(alpha_unitizer, vmax);
+
+  double t = 0;
+  ::Eigen::Matrix<double, 4, 1> X;
+  {
+    ::Eigen::Matrix<double, 2, 1> theta_t = trajectory.ThetaT(0.0);
+    X << theta_t(0), 0.0, theta_t(1), 0.0;
+  }
+
+  TrajectoryFollower follower(&path, &trajectory, alpha_unitizer);
+  constexpr double sim_dt = 0.00505;
+  while (t < 1.0) {
+    follower.Update(X, sim_dt, vmax);
+    X = Dynamics::UnboundedDiscreteDynamics(X, follower.U(), sim_dt);
+    t += sim_dt;
+  }
+
+  ::Eigen::Matrix<double, 4, 1> final_X;
+  ::Eigen::Matrix<double, 2, 1> final_theta_t =
+      trajectory.ThetaT(path.length());
+  final_X << final_theta_t(0), 0.0, final_theta_t(1), 0.0;
+
+  // Verify that we got to the end.
+  EXPECT_TRUE(X.isApprox(final_X, 0.01))
+      << ": X is " << X.transpose() << " final_X is " << final_X.transpose();
+
+  // Verify that our goal is at the end.
+  EXPECT_TRUE(final_theta_t.isApprox(path.Theta(follower.goal(0))));
+}
+
 }  // namespace testing
 }  // namespace arm
 }  // namespace superstructure