commit 4037bc7c6348c4c25aaff412c741dc567320f053
author milind-u <milind.upadhyay@gmail.com> Wed Feb 22 21:39:40 2023 -0800
committer Milind Upadhyay <milind.upadhyay@gmail.com> Wed Feb 22 21:55:20 2023 -0800
tree 3ed894e386e0a28490ec3a742984212577a99778
parent 76dd4e7a501bbf1e7be77dc15262874867ef2eac

Actually check roll joint collision for generating

Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: If2a53af11548dd5b6979e7298d01d54e480982dc

y2023/control_loops/python/graph_codegen.py

diff --git a/y2023/control_loops/python/graph_codegen.py b/y2023/control_loops/python/graph_codegen.py
index d056d20..054a32d 100644
--- a/y2023/control_loops/python/graph_codegen.py
+++ b/y2023/control_loops/python/graph_codegen.py
@@ -13,6 +13,8 @@
 
 
 def add_edge(cc_file, name, segment, index, reverse):
+    segment.VerifyPoints()
+
     cc_file.append("  // Adding edge %d" % index)
     vmax = "vmax"
     if segment.vmax:

y2023/control_loops/python/graph_paths.py

diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 2a5ef73..87ced83 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -22,7 +22,7 @@
                                                   1.2,
                                                   np.pi / 2.0,
                                                   circular_index=0)
-neutral_to_score_control_alpha_rolls = [(0.33, 0.0), (.95, np.pi / 2.0)]
+neutral_to_score_control_alpha_rolls = [(0.40, 0.0), (.95, np.pi / 2.0)]
 
 # TODO(Max): Add real paths for arm.
 points = [(neutral, "NeutralPos"), (pickup_pos, "PickupPos"),

y2023/control_loops/python/graph_tools.py

diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index cf4fd7b..fef1083 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -409,12 +409,11 @@
 
         self.DoDrawTo(cr, theta_version)
 
-    def ToThetaPoints(self):
+    def VerifyPoints(self):
         points = self.DoToThetaPoints()
         if self.roll_joint_collision(points, verbose=True) or \
            self.arm_past_limit(points, verbose=True):
             sys.exit(1)
-        return points
 
 
 class SplineSegmentBase(Path):