diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index 6104568..57b60e6 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -95,3 +95,18 @@
         "//third_party/eigen",
     ],
 )
+
+cc_library(
+    name = "graph",
+    srcs = ["graph.cc"],
+    hdrs = ["graph.h"],
+)
+
+cc_test(
+    name = "graph_test",
+    srcs = ["graph_test.cc"],
+    deps = [
+        ":graph",
+        "//aos/testing:googletest",
+    ],
+)
diff --git a/y2018/control_loops/superstructure/arm/graph.cc b/y2018/control_loops/superstructure/arm/graph.cc
new file mode 100644
index 0000000..a1e6fb2
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/graph.cc
@@ -0,0 +1,65 @@
+#include "y2018/control_loops/superstructure/arm/graph.h"
+
+#include <assert.h>
+#include <algorithm>
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+SearchGraph::SearchGraph(size_t num_vertexes, const std::vector<Edge> &edges)
+    : edges_(edges) {
+  vertexes_.resize(num_vertexes);
+  size_t i = 0;
+  for (const auto &edge : edges) {
+    assert(edge.start < num_vertexes);
+    assert(edge.end < num_vertexes);
+    assert(edge.start != edge.end);
+    assert(edge.cost > 0);
+    vertexes_[edge.start].forward.emplace_back(HalfEdge{edge.end, i});
+    vertexes_[edge.end].reverse.emplace_back(HalfEdge{edge.start, i});
+    ++i;
+  }
+  for (auto &vertex : vertexes_) {
+    std::sort(vertex.forward.begin(), vertex.forward.end());
+    std::sort(vertex.reverse.begin(), vertex.reverse.end());
+  }
+  vertex_heap_.Reserve(num_vertexes);
+}
+
+SearchGraph::~SearchGraph() {}
+
+void SearchGraph::SetGoal(size_t vertex) {
+  assert(vertex < vertexes_.size());
+  vertex_heap_.Clear();
+
+  for (auto &vertex : vertexes_) {
+    vertex.cached_distance = std::numeric_limits<double>::infinity();
+  }
+
+  vertexes_[vertex].cached_distance = 0.0;
+  vertex_heap_.InsertOrDecrease(&vertexes_[vertex]);
+
+  while (!vertex_heap_.Empty()) {
+    Vertex *vertex = vertex_heap_.PopMin();
+    for (const auto &half_edge : vertex->reverse) {
+      Vertex *to_adjust = &vertexes_[half_edge.dest];
+      double adjust_cost = vertex->cached_distance + GetCost(half_edge);
+      if (adjust_cost < to_adjust->cached_distance) {
+        to_adjust->cached_distance = adjust_cost;
+        vertex_heap_.InsertOrDecrease(to_adjust);
+      }
+    }
+  }
+}
+
+double SearchGraph::GetCostToGoal(size_t vertex) {
+  assert(vertex < vertexes_.size());
+  return vertexes_[vertex].cached_distance;
+}
+
+}  // namespace arm
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/graph.h b/y2018/control_loops/superstructure/arm/graph.h
new file mode 100644
index 0000000..1c2f9e5
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/graph.h
@@ -0,0 +1,179 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
+
+#include <assert.h>
+#include <stdlib.h>
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <vector>
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+// Grr... normal priority queues don't allow modifying the node cost.
+// This relies on pointer stability for the passed in T.
+template <typename T>
+class IntrusivePriorityQueue {
+ public:
+  // T is expected to be like so:
+  //
+  // struct T {
+  //   IntrusivePriorityQueue<T>::QueueEntry* entry_ = nullptr;
+  // };
+  //
+  // This QueueEntry is like a unique_ptr that always maintains a link
+  // back to the unique_ptr pointing to that entry. This maintains the
+  // mapping between the elements and their indexes in the list.
+  // This mapping is crucial for supporting O(log(n)) decrease-key
+  // operations.
+  class QueueEntry {
+   public:
+    QueueEntry(T *value) : value_(value) { value_->entry_ = this; }
+
+    QueueEntry(QueueEntry &&o) : value_(o.value_) {
+      if (value_) {
+        value_->entry_ = this;
+      }
+      o.value_ = nullptr;
+    }
+
+    QueueEntry &operator=(QueueEntry &&o) {
+      if (this == &o) return *this;
+      std::swap(value_, o.value_);
+      if (value_) value_->entry_ = this;
+      if (o.value_) o.value_->entry_ = this;
+      return *this;
+    }
+
+    ~QueueEntry() {
+      if (value_) value_->entry_ = nullptr;
+    }
+
+    T *get() { return value_; }
+
+    bool operator<(const QueueEntry &o) { return *value_ < *o.value_; }
+
+   private:
+    friend class IntrusivePriorityQueue;
+    T *value_ = nullptr;
+  };
+
+  // Conceptually, nodes start at inifinity and can only decrease in cost.
+  // The infinity nodes are not in the queue.
+  void InsertOrDecrease(T *t) {
+    if (t->entry_ != nullptr) {
+      std::push_heap(queue_.begin(), queue_.begin() + (GetIndex(t) + 1));
+    } else {
+      queue_.emplace_back(t);
+      std::push_heap(queue_.begin(), queue_.end());
+    }
+  }
+
+  // For testing.
+  void VerifyCorrectness() {
+    for (size_t i = 0; i < queue_.size(); ++i) {
+      assert(queue_[i].value_->entry_ == &queue_[i]);
+    }
+
+    assert(std::is_heap(queue_.begin(), queue_.end()));
+  }
+
+  T *PopMin() {
+    T *result = queue_.front().get();
+    std::pop_heap(queue_.begin(), queue_.end());
+    queue_.pop_back();
+    return result;
+  }
+
+  bool Empty() const { return queue_.empty(); }
+
+  void Clear() { queue_.clear(); }
+
+  void Reserve(size_t n) { queue_.reserve(n); }
+
+ private:
+  size_t GetIndex(T *t) { return t->entry_ - &queue_[0]; }
+
+  std::vector<QueueEntry> queue_;
+};
+
+// You pass in a graph (with num_vertexes) and a set of edges described by
+// edges.
+// Then, this will search that graph to find the optimal path from all points
+// to a goal point.
+//
+// A traversal algorithm would be as such:
+// // Initialize graph...
+// graph.SetGoal(my_goal);
+// size_t current_node = node;
+//
+// while (current_node != my_goal) {
+//   edge = argmin(GetCostToGoal(edge.dest) for edge in
+//   Neighbors(current_node));
+//   Travel along edge # edge.edge_id.
+//   current_node = edge.dest;
+// }
+class SearchGraph {
+ public:
+  struct Edge {
+    size_t start;
+    size_t end;
+    double cost;
+  };
+
+  // The part of an edge stored at every vertex.
+  struct HalfEdge {
+    size_t dest;
+    // This id matches the corresponding id in the edges array from the
+    // constructor.
+    size_t edge_id;
+    bool operator<(const HalfEdge &o) const { return dest < o.dest; }
+  };
+
+  SearchGraph(size_t num_vertexes, const std::vector<Edge> &edges);
+  ~SearchGraph();
+
+  // Set the goal vertex.
+  void SetGoal(size_t vertex);
+
+  // Compute the cost if you're at vertex.
+  double GetCostToGoal(size_t vertex);
+
+  // For walking the graph yourself.
+  const std::vector<HalfEdge> &Neighbors(size_t vertex) {
+    assert(vertex < vertexes_.size());
+    return vertexes_[vertex].forward;
+  }
+
+  size_t num_vertexes() const { return vertexes_.size(); }
+
+ private:
+  size_t goal_vertex_ = std::numeric_limits<size_t>::max();
+  struct Vertex {
+    std::vector<HalfEdge> forward;
+    std::vector<HalfEdge> reverse;
+    double cached_distance = std::numeric_limits<double>::infinity();
+
+    bool operator<(const Vertex &o) {
+      return cached_distance > o.cached_distance;
+    }
+
+    IntrusivePriorityQueue<Vertex>::QueueEntry *entry_ = nullptr;
+  };
+
+  double GetCost(const HalfEdge &edge) { return edges_[edge.edge_id].cost; }
+  std::vector<Vertex> vertexes_;
+  std::vector<Edge> edges_;
+
+  IntrusivePriorityQueue<Vertex> vertex_heap_;
+};
+
+}  // namespace arm
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2018
+
+#endif  // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
diff --git a/y2018/control_loops/superstructure/arm/graph_test.cc b/y2018/control_loops/superstructure/arm/graph_test.cc
new file mode 100644
index 0000000..1f04be2
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/graph_test.cc
@@ -0,0 +1,78 @@
+#include "y2018/control_loops/superstructure/arm/graph.h"
+
+#include "gtest/gtest.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+namespace testing {
+
+class HeapTest {
+ public:
+  explicit HeapTest(size_t id, double cost) : id_(id), cost_(cost) {}
+
+  void SetCost(double cost) { cost_ = cost; }
+
+  bool operator<(const HeapTest &o) {
+    bool value = cost_ > o.cost_;
+    // printf("compare: %zu - %g < %zu %g -> %s\n", id_, cost_, o.id_, o.cost_,
+    // value ? "true" : "false");
+    return value;
+  }
+
+ private:
+  size_t id_;
+  double cost_;
+
+  IntrusivePriorityQueue<HeapTest>::QueueEntry *entry_ = nullptr;
+  friend class IntrusivePriorityQueue<HeapTest>;
+};
+
+TEST(IntrusivePriorityQueue, HeapTest) {
+  std::vector<HeapTest> items;
+  IntrusivePriorityQueue<HeapTest> queue;
+  size_t i = 0;
+  for (double value :
+       {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0}) {
+    items.push_back(HeapTest(i, value));
+    ++i;
+  }
+  for (auto &item : items) {
+    queue.InsertOrDecrease(&item);
+    queue.VerifyCorrectness();
+  }
+
+  items[6].SetCost(-1.0);
+  queue.InsertOrDecrease(&items[6]);
+  queue.VerifyCorrectness();
+
+  while (!queue.Empty()) {
+    queue.PopMin();
+    queue.VerifyCorrectness();
+  }
+}
+
+TEST(GraphTest, Distances) {
+  SearchGraph graph(6, {{0, 1, 4.0},
+                        {1, 5, 2.0},
+                        {2, 0, 7.0},
+                        {2, 3, 8.0},
+                        {3, 1, 2.0},
+                        {4, 0, 1.0},
+                        {5, 2, 9.0},
+                        {5, 4, 8.0}});
+
+  graph.SetGoal(2);
+  size_t i = 0;
+  for (double expected_cost : {15, 11, 0, 13, 16, 9}) {
+    EXPECT_EQ(graph.GetCostToGoal(i), expected_cost);
+    ++i;
+  }
+}
+
+}  // namespace testing
+}  // namespace arm
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2018