Adding graph routines for the arm path planning.
This is guaranteed not to perform any runtime allocations, and
allows constructing a list of vertexes, and directed edges with costs
and then traversing this graph optimally based on those costs.
Change-Id: I17614b794c3ec5c3e1569cc75a030acf40a942b0
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