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/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_