Codegen paths from python for the arm and build the graph
We now have a bunch of really cool paths in C++. They look pretty good.
Change-Id: I9590d771cebc64cd520ceed2617aca8a82f78362
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
new file mode 100644
index 0000000..2df9a33
--- /dev/null
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -0,0 +1,175 @@
+from __future__ import print_function
+import sys
+import numpy
+import graph_generate
+
+
+def index_function_name(name):
+ return "%sIndex" % name
+
+
+def path_function_name(name):
+ return "Make%sPath" % name
+
+
+def add_edge(cc_file, name, segment, index, reverse):
+ cc_file.append(" // Adding edge %d" % index)
+ if reverse:
+ cc_file.append(
+ " trajectories->emplace_back(Path::Reversed(%s()), 0.005);" %
+ (path_function_name(str(name))))
+ else:
+ cc_file.append(
+ " trajectories->emplace_back(%s(), 0.005);" %
+ (path_function_name(str(name))))
+
+ start_index = None
+ end_index = None
+ for point, name in graph_generate.points:
+ if (point == segment.start).all():
+ start_index = name
+ if (point == segment.end).all():
+ end_index = name
+
+ if reverse:
+ start_index, end_index = end_index, start_index
+
+ cc_file.append(" edges.push_back(SearchGraph::Edge{%s(), %s()," %
+ (index_function_name(start_index),
+ index_function_name(end_index)))
+ cc_file.append(
+ " trajectories->back().path().length()});"
+ )
+
+ # TODO(austin): Allow different vmaxes for different paths.
+ cc_file.append(
+ " trajectories->back().OptimizeTrajectory(alpha_unitizer, vmax);"
+ )
+ cc_file.append("")
+
+
+def main(argv):
+ cc_file = []
+ cc_file.append(
+ "#include \"y2018/control_loops/superstructure/arm/generated_graph.h\""
+ )
+ cc_file.append("")
+ cc_file.append("#include <memory>")
+ cc_file.append("")
+ cc_file.append(
+ "#include \"y2018/control_loops/superstructure/arm/trajectory.h\"")
+ cc_file.append(
+ "#include \"y2018/control_loops/superstructure/arm/graph.h\"")
+ cc_file.append("")
+ cc_file.append("namespace y2018 {")
+ cc_file.append("namespace control_loops {")
+ cc_file.append("namespace superstructure {")
+ cc_file.append("namespace arm {")
+
+ h_file = []
+ h_file.append(
+ "#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GENERATED_GRAPH_H_")
+ h_file.append(
+ "#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GENERATED_GRAPH_H_")
+ h_file.append("")
+ h_file.append("#include <memory>")
+ h_file.append("")
+ h_file.append(
+ "#include \"y2018/control_loops/superstructure/arm/trajectory.h\"")
+ h_file.append(
+ "#include \"y2018/control_loops/superstructure/arm/graph.h\"")
+ h_file.append("")
+ h_file.append("namespace y2018 {")
+ h_file.append("namespace control_loops {")
+ h_file.append("namespace superstructure {")
+ h_file.append("namespace arm {")
+
+ # Now dump out the vertices and associated constexpr vertex name functions.
+ for index, point in enumerate(graph_generate.points):
+ h_file.append("")
+ h_file.append("constexpr uint32_t %s() { return %d; }" %
+ (index_function_name(point[1]), index))
+ h_file.append(
+ "inline ::Eigen::Matrix<double, 2, 1> %sPoint() {" % point[1])
+ h_file.append(
+ " return (::Eigen::Matrix<double, 2, 1>() << %f, %f).finished();"
+ % (numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
+ h_file.append("}")
+
+ # Add the Make*Path functions.
+ h_file.append("")
+ cc_file.append("")
+ for name, segment in list(enumerate(graph_generate.unnamed_segments)) + [
+ (x.name, x) for x in graph_generate.named_segments
+ ]:
+ h_file.append(
+ "::std::unique_ptr<Path> %s();" % path_function_name(name))
+ cc_file.append(
+ "::std::unique_ptr<Path> %s() {" % path_function_name(name))
+ cc_file.append(" return ::std::unique_ptr<Path>(new Path({")
+ for point in segment.ToThetaPoints():
+ cc_file.append(" {{%.12f, %.12f, %.12f," %
+ (numpy.pi / 2.0 - point[0],
+ numpy.pi / 2.0 - point[1], -point[2]))
+ cc_file.append(" %.12f, %.12f, %.12f}}," %
+ (-point[3], -point[4], -point[5]))
+ cc_file.append(" }));")
+ cc_file.append("}")
+
+ # Now create the MakeSearchGraph function.
+ h_file.append("")
+ h_file.append("// Builds a search graph.")
+ h_file.append("SearchGraph MakeSearchGraph("
+ "::std::vector<Trajectory> *trajectories,")
+ h_file.append(" "
+ "const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
+ h_file.append(" double vmax);")
+ cc_file.append("SearchGraph MakeSearchGraph("
+ "::std::vector<Trajectory> *trajectories,")
+ cc_file.append(" "
+ "const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
+ cc_file.append(" " "double vmax) {")
+ cc_file.append(" ::std::vector<SearchGraph::Edge> edges;")
+
+ index = 0
+ segments_and_names = list(enumerate(graph_generate.unnamed_segments)) + [
+ (x.name, x) for x in graph_generate.named_segments
+ ]
+
+ for name, segment in segments_and_names:
+ add_edge(cc_file, name, segment, index, False)
+ index += 1
+ add_edge(cc_file, name, segment, index, True)
+ index += 1
+
+ cc_file.append(" return SearchGraph(%d, ::std::move(edges));" % len(
+ graph_generate.points))
+ cc_file.append("}")
+
+ h_file.append("")
+ h_file.append("} // namespace arm")
+ h_file.append("} // namespace superstructure")
+ h_file.append("} // namespace control_loops")
+ h_file.append("} // namespace y2018")
+ h_file.append("")
+ h_file.append(
+ "#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GENERATED_GRAPH_H_")
+
+ cc_file.append("} // namespace arm")
+ cc_file.append("} // namespace superstructure")
+ cc_file.append("} // namespace control_loops")
+ cc_file.append("} // namespace y2018")
+
+ if len(argv) == 3:
+ with open(argv[1], "w") as hfd:
+ hfd.write("\n".join(h_file))
+
+ with open(argv[2], "w") as ccfd:
+ ccfd.write("\n".join(cc_file))
+ else:
+ print("\n".join(h_file))
+ print("\n".join(cc_file))
+
+
+if __name__ == '__main__':
+ main(sys.argv)