commit b874fd31b051452444bc3094030333e124c39c22
author Austin Schuh <austin.linux@gmail.com> Mon Mar 05 00:27:10 2018 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 10 19:46:04 2018 -0800
tree 9fdbda2d698c25abe047cd61245f366df20105bd
parent f88d3acc1a2d9b0ce6a4d57864d3da17180e93b3

Sped box grab up significantly and handle disable

Add some more trajectories and cut tolerances to make box grabbing go
faster.  Also, go to the nearest node when disabled.

Change-Id: Ic641db69a8c18eb61771ebc352e05ceef0d90084

y2018/control_loops/python/graph_codegen.py

diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index fbdcfbb..d5dbe12 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -19,9 +19,8 @@
             "  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))))
+        cc_file.append("  trajectories->emplace_back(%s(), 0.005);" %
+                       (path_function_name(str(name))))
 
     start_index = None
     end_index = None
@@ -38,13 +37,11 @@
                    (index_function_name(start_index),
                     index_function_name(end_index)))
     cc_file.append(
-        "                     trajectories->back().path().length()});"
-    )
+        "                     (trajectories->back().path().length() + 0.2)});")
 
     # TODO(austin): Allow different vmaxes for different paths.
     cc_file.append(
-        "  trajectories->back().OptimizeTrajectory(alpha_unitizer, vmax);"
-    )
+        "  trajectories->back().OptimizeTrajectory(alpha_unitizer, vmax);")
     cc_file.append("")
 
 
@@ -115,16 +112,17 @@
                            (-point[3], -point[4], -point[5]))
         cc_file.append("  }));")
         cc_file.append("}")
-    
+
     # Matrix of nodes
     h_file.append("::std::vector<::Eigen::Matrix<double, 2, 1>> PointList();")
 
-    cc_file.append("::std::vector<::Eigen::Matrix<double, 2, 1>> PointList() {")
+    cc_file.append(
+        "::std::vector<::Eigen::Matrix<double, 2, 1>> PointList() {")
     cc_file.append("  ::std::vector<::Eigen::Matrix<double, 2, 1>> points;")
-    for index, point in enumerate(graph_generate.points):
+    for point in graph_generate.points:
         cc_file.append(
-            "  points.push_back((::Eigen::Matrix<double, 2, 1>() << %.12s, %.12s).finished());" % (
-                numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
+            "  points.push_back((::Eigen::Matrix<double, 2, 1>() << %.12s, %.12s).finished());"
+            % (numpy.pi / 2.0 - point[0][0], numpy.pi / 2.0 - point[0][1]))
     cc_file.append("  return points;")
     cc_file.append("}")
 

y2018/control_loops/python/graph_generate.py

diff --git a/y2018/control_loops/python/graph_generate.py b/y2018/control_loops/python/graph_generate.py
index fdc5507..607613c 100644
--- a/y2018/control_loops/python/graph_generate.py
+++ b/y2018/control_loops/python/graph_generate.py
@@ -387,7 +387,7 @@
 
 
 tall_box_x = 0.401
-tall_box_y = 0.13
+tall_box_y = 0.14
 
 short_box_x = 0.431
 short_box_y = 0.082
@@ -459,6 +459,10 @@
     XYSegment(tall_box_grab, short_box_grab, "TallToShortBox"),
     SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
                   ready_above_box, "ReadyToNeutral"),
+    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+                  tall_box_grab, "TallToNeutral"),
+    SplineSegment(neutral, ready_above_box_c1, ready_above_box_c2,
+                  short_box_grab, "ShortToNeutral"),
     SplineSegment(neutral, up_c1, up_c2, up, "NeutralToUp"),
     SplineSegment(neutral, front_high_box_c1, front_high_box_c2,
                   front_high_box, "NeutralToFrontHigh"),