Make arm UI angle conventions match C++
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I106f138949abd0bcda18546ebcc12407accec900
diff --git a/y2023/control_loops/python/graph_codegen.py b/y2023/control_loops/python/graph_codegen.py
index 54b77a9..69e2497 100644
--- a/y2023/control_loops/python/graph_codegen.py
+++ b/y2023/control_loops/python/graph_codegen.py
@@ -152,8 +152,7 @@
point[1])
h_file.append(
" return (::Eigen::Matrix<double, 3, 1>() << %f, %f, %f).finished();"
- % (np.pi / 2.0 - point[0][0], np.pi / 2.0 - point[0][1],
- np.pi / 2.0 - point[0][2]))
+ % (point[0][0], point[0][1], point[0][2]))
h_file.append("}")
front_points = [
@@ -195,16 +194,17 @@
segment.start, segment.control1, segment.control2, segment.end
]
for i in range(len(points)):
- cc_file.append("%.12f," % (np.pi / 2.0 - points[i][0]))
+ cc_file.append("%.12f," % (points[i][0]))
for i in range(len(points)):
- cc_file.append("%.12f%s" % (np.pi / 2.0 - points[i][1],
- ", " if i != len(points) - 1 else ""))
+ cc_file.append(
+ "%.12f%s" %
+ (points[i][1], ", " if i != len(points) - 1 else ""))
cc_file.append(").finished()), {")
for alpha, roll in segment.alpha_rolls:
cc_file.append(
"CosSpline::AlphaTheta{.alpha = %.12f, .theta = %.12f}" %
- (alpha, np.pi / 2.0 - roll))
+ (alpha, roll))
if alpha != segment.alpha_rolls[-1][0]:
cc_file.append(", ")
cc_file.append(" }}));")
@@ -219,8 +219,7 @@
for point in graph_paths.points:
cc_file.append(
" points.push_back((::Eigen::Matrix<double, 3, 1>() << %.12s, %.12s, %.12s).finished());"
- % (np.pi / 2.0 - point[0][0], np.pi / 2.0 - point[0][1],
- np.pi / 2.0 - point[0][2]))
+ % (point[0][0], point[0][1], point[0][2]))
cc_file.append(" return points;")
cc_file.append("}")
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index c60a2c5..72dc02f 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -4,24 +4,24 @@
neutral = to_theta_with_circular_index_and_roll(joint_center[0],
joint_center[1] + l2 - l1,
- np.pi / 2,
+ 0.0,
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,
- np.pi / 2,
+ 0.0,
circular_index=-1)
-neutral_to_pickup_control_alpha_rolls = [(0.33, np.pi / 2), (.67, np.pi / 2)]
+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)
score_pos = to_theta_with_circular_index_and_roll(-1.0,
1.2,
- np.pi / 2,
+ 0.0,
circular_index=-1)
-neutral_to_score_control_alpha_rolls = [(0.33, np.pi / 2), (.67, np.pi / 2)]
+neutral_to_score_control_alpha_rolls = [(0.33, 0.0), (.67, 0.0)]
# TODO(Max): Add real paths for arm.
points = [(neutral, "NeutralPos"), (pickup_pos, "PickupPos"),
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index 3b5048e..feb611a 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -17,6 +17,15 @@
theta_end_circle_size = 0.07
+# Shift the angle between the convention used for input/output and the convention we use for some computations here
+def shift_angle(theta):
+ return np.pi / 2 - theta
+
+
+def shift_angles(thetas):
+ return [shift_angle(theta) for theta in thetas]
+
+
# Convert from x-y coordinates to theta coordinates.
# orientation is a bool. This orientation is circular_index mod 2.
# where circular_index is the circular index, or the position in the
@@ -154,6 +163,10 @@
def roll_joint_collision(theta1, theta2, theta3):
+ theta1 = shift_angle(theta1)
+ theta2 = shift_angle(theta2)
+ theta3 = shift_angle(theta3)
+
end_effector_points = to_end_effector_points(theta1, theta2, theta3)
assert len(end_effector_points) == 8 and len(end_effector_points[0]) == 3
@@ -181,8 +194,8 @@
def get_xy(theta):
- theta1 = theta[0]
- theta2 = theta[1]
+ theta1 = shift_angle(theta[0])
+ theta2 = shift_angle(theta[1])
x = np.cos(theta1) * l1 + np.cos(theta2) * l2 + joint_center[0]
y = np.sin(theta1) * l1 + np.sin(theta2) * l2 + joint_center[1]
return np.array((x, y))
@@ -210,7 +223,7 @@
theta1, theta2 = to_theta((x, y), circular_index)
n_circular_index = int(np.floor((theta2 - theta1) / np.pi))
theta2 = theta2 + ((circular_index - n_circular_index)) * np.pi
- return np.array((theta1, theta2))
+ return np.array((shift_angle(theta1), shift_angle(theta2)))
# to_theta, but distinguishes between
@@ -415,8 +428,10 @@
def DoDrawTo(self, cr, theta_version):
if (theta_version):
draw_lines(cr, [
- spline_eval(self.start, self.control1, self.control2, self.end,
- alpha) for alpha in subdivide_multistep()
+ shift_angles(
+ spline_eval(self.start, self.control1, self.control2,
+ self.end, alpha))
+ for alpha in subdivide_multistep()
])
else:
start = get_xy(self.start)