Add limit checks to arm UI
Constrain the relative angle between the proximal and distal joint.
Also constrain roll angle.
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I195bdac60081ceb2e3cf39cf3546a886b004e6c6
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 72dc02f..b21f548 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -5,22 +5,22 @@
neutral = to_theta_with_circular_index_and_roll(joint_center[0],
joint_center[1] + l2 - l1,
0.0,
- circular_index=-1)
+ circular_index=1)
-neutral_to_pickup_1 = to_theta_with_circular_index(0.3, 0.6, circular_index=-1)
-neutral_to_pickup_2 = to_theta_with_circular_index(0.3, 0.4, circular_index=-1)
+neutral_to_pickup_1 = to_theta_with_circular_index(0.3, 0.6, circular_index=1)
+neutral_to_pickup_2 = to_theta_with_circular_index(0.3, 0.4, circular_index=1)
pickup_pos = to_theta_with_circular_index_and_roll(0.6,
0.1,
0.0,
- circular_index=-1)
+ circular_index=1)
neutral_to_pickup_control_alpha_rolls = [(0.33, 0.0), (.67, 0.0)]
-neutral_to_score_1 = to_theta_with_circular_index(-0.4, 1.2, circular_index=-1)
-neutral_to_score_2 = to_theta_with_circular_index(-0.7, 1.2, circular_index=-1)
+neutral_to_score_1 = to_theta_with_circular_index(-0.4, 1.2, circular_index=1)
+neutral_to_score_2 = to_theta_with_circular_index(-0.7, 1.2, circular_index=1)
score_pos = to_theta_with_circular_index_and_roll(-1.0,
1.2,
0.0,
- circular_index=-1)
+ circular_index=1)
neutral_to_score_control_alpha_rolls = [(0.33, 0.0), (.67, 0.0)]
# TODO(Max): Add real paths for arm.
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index feb611a..4083906 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -189,6 +189,28 @@
return collision
+# Delta limit means theta2 - theta1.
+# The limit for the proximal and distal is relative,
+# so define constraints for this delta.
+UPPER_DELTA_LIMIT = 0.0
+LOWER_DELTA_LIMIT = -1.9 * np.pi
+
+# TODO(milind): put actual proximal limits
+UPPER_PROXIMAL_LIMIT = np.pi * 1.5
+LOWER_PROXIMAL_LIMIT = -np.pi
+
+UPPER_ROLL_JOINT_LIMIT = 0.75 * np.pi
+LOWER_ROLL_JOINT_LIMIT = -0.75 * np.pi
+
+
+def arm_past_limit(theta1, theta2, theta3):
+ delta = theta2 - theta1
+ return (delta > UPPER_DELTA_LIMIT or delta < LOWER_DELTA_LIMIT) or (
+ theta3 > UPPER_ROLL_JOINT_LIMIT or
+ theta3 < LOWER_ROLL_JOINT_LIMIT) or (theta1 > UPPER_PROXIMAL_LIMIT
+ or theta1 < LOWER_PROXIMAL_LIMIT)
+
+
def get_circular_index(theta):
return int(np.floor((theta[1] - theta[0]) / np.pi))
@@ -361,14 +383,30 @@
return True
return False
+ def arm_past_limit(self, points, verbose=True):
+ for point in points:
+ if arm_past_limit(*point):
+ if verbose:
+ print("Arm past limit for path %s in point %s" %
+ (self.name, point))
+ return True
+ return False
+
def DrawTo(self, cr, theta_version):
- if self.roll_joint_collision(self.DoToThetaPoints()):
+ points = self.DoToThetaPoints()
+ if self.roll_joint_collision(points):
+ # Draw the spline red
cr.set_source_rgb(1.0, 0.0, 0.0)
+ elif self.arm_past_limit(points):
+ # Draw the spline orange
+ cr.set_source_rgb(1.0, 0.5, 0.0)
+
self.DoDrawTo(cr, theta_version)
def ToThetaPoints(self):
points = self.DoToThetaPoints()
- if self.roll_joint_collision(points, verbose=True):
+ if self.roll_joint_collision(points, verbose=True) or \
+ self.arm_past_limit(points, verbose=True):
sys.exit(1)
return points