Add cube pickup path
Turns out the link lengths in python didn't match C++... Sigh. I
updated all the trajectories to be equivilent to what they were before
the constant switch.
Change-Id: I9d29c51b6c6dc4ecc95b7182ac8bdd1949806daa
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index c017656..66ca158 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -2,34 +2,41 @@
from y2023.control_loops.python.graph_tools import *
-neutral = to_theta_with_circular_index_and_roll(joint_center[0],
- joint_center[1] + l2 - l1,
- 0.0,
- circular_index=1)
+neutral = np.array((np.pi, 0.0, 0.0))
# NeutralToGroundPickupBackConeUp
neutral_to_cone_up_1 = np.array([3.170156, -0.561227])
neutral_to_cone_up_2 = np.array([2.972776, -1.026820])
ground_pickup_back_cone_up = to_theta_with_circular_index_and_roll(
- -0.913162844198605, 0.35, np.pi / 2.0, circular_index=1)
+ -1.07774334, 0.36308701, np.pi / 2.0, circular_index=1)
# NeutralToGroundPickupBackConeDown
neutral_to_ground_pickup_back_cone_down_1 = np.array([3.170156, -0.561227])
neutral_to_ground_pickup_back_cone_down_2 = np.array([2.972776, -1.026820])
ground_pickup_back_cone_down = to_theta_with_circular_index_and_roll(
- -0.95, 0.24, np.pi / 2.0, circular_index=1)
+ -1.11487594, 0.23140145, np.pi / 2.0, circular_index=1)
+
+# NeutralToGroundPickupBackCube
+neutral_to_ground_pickup_back_cube_1 = np.array([3.153228, -0.497009])
+neutral_to_ground_pickup_back_cube_2 = np.array([2.972776, -1.026820])
+neutral_to_hp_pickup_back_cube_alpha_rolls = [
+ (0.7, 0.0),
+ (.9, -np.pi / 2.0),
+]
+ground_pickup_back_cube = to_theta_with_circular_index_and_roll(
+ -1.102, 0.224, -np.pi / 2.0, circular_index=1)
# NeutralToBackMidConeUpScore
neutral_to_score_back_mid_cone_up_1 = np.array([0.994244, -1.417442])
neutral_to_score_back_mid_cone_up_2 = np.array([1.711325, -0.679748])
score_back_mid_cone_up_pos = to_theta_with_circular_index_and_roll(
- -1.255555, 1.10, np.pi / 2.0, circular_index=0)
+ -1.41871454, 1.07476162, np.pi / 2.0, circular_index=0)
# NeutralToMidConeDownScore
neutral_to_score_mid_cone_down_1 = np.array([3.394572, -0.239378])
neutral_to_score_mid_cone_down_2 = np.array([3.654854, -0.626835])
score_mid_cone_down_pos = to_theta_with_circular_index_and_roll(
- -1.23, 0.74, np.pi / 2.0, circular_index=1)
+ -1.37792406, 0.81332449, np.pi / 2.0, circular_index=1)
# NeutralToMidConeDownScore
neutral_to_hp_pickup_back_cone_up_1 = np.array([2.0, -0.239378])
@@ -39,19 +46,19 @@
(.9, np.pi / 2.0),
]
hp_pickup_back_cone_up = to_theta_with_circular_index_and_roll(
- -0.94, 1.31, np.pi / 2.0, circular_index=0)
+ -1.1050539, 1.31390128, np.pi / 2.0, circular_index=0)
# NeutralToFrontHighConeUpScore
neutral_to_score_front_high_cone_up_1 = np.array([2.594244, 0.417442])
neutral_to_score_front_high_cone_up_2 = np.array([1.51325, 0.679748])
score_front_high_cone_up_pos = to_theta_with_circular_index_and_roll(
- 0.87, 1.26, -np.pi / 2.0, circular_index=0)
+ 0.98810344, 1.37536719, -np.pi / 2.0, circular_index=0)
# NeutralToFrontMidConeUpScore
neutral_to_score_front_mid_cone_up_1 = np.array([3.0, 0.317442])
neutral_to_score_front_mid_cone_up_2 = np.array([2.9, 0.479748])
score_front_mid_cone_up_pos = to_theta_with_circular_index_and_roll(
- 0.34, 0.93, -np.pi / 2.0, circular_index=0)
+ 0.43740453, 1.06330555, -np.pi / 2.0, circular_index=0)
neutral_to_cone_down_1 = np.array([2.396694, 0.508020])
neutral_to_cone_down_2 = np.array([2.874513, 0.933160])
@@ -104,6 +111,7 @@
points = [(neutral, "NeutralPos"),
(ground_pickup_back_cone_up, "GroundPickupBackConeUp"),
(ground_pickup_back_cone_down, "GroundPickupBackConeDown"),
+ (ground_pickup_back_cube, "GroundPickupBackCube"),
(hp_pickup_back_cone_up, "HPPickupBackConeUp"),
(cone_down_pos, "ConeDownPos"), (score_low_pos, "ScoreLowPos"),
(score_back_mid_cone_up_pos, "ScoreBackMidConeUpPos"),
@@ -126,6 +134,11 @@
neutral_to_ground_pickup_back_cone_down_2,
ground_pickup_back_cone_down,
neutral_to_pickup_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToGroundPickupBackCube", neutral,
+ neutral_to_ground_pickup_back_cube_1,
+ neutral_to_ground_pickup_back_cube_2,
+ ground_pickup_back_cube,
+ neutral_to_hp_pickup_back_cube_alpha_rolls),
ThetaSplineSegment("NeutralToHPPickupBackConeUp", neutral,
neutral_to_hp_pickup_back_cone_up_1,
neutral_to_hp_pickup_back_cone_up_2,
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index 769c7b3..8751935 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -9,7 +9,7 @@
# Joint distances (l1 = "proximal", l2 = "distal")
l1 = 20.0 * IN_TO_M
-l2 = 31.5 * IN_TO_M
+l2 = 38.0 * IN_TO_M
max_dist = 0.01
max_dist_theta = np.pi / 64
@@ -35,7 +35,7 @@
orient = (circular_index % 2) == 0
x = pt[0]
y = pt[1]
- x -= joint_center[0]
+ x -= joint_center[0] - 1e-9
y -= joint_center[1]
l3 = np.hypot(x, y)
t3 = np.arctan2(y, x)