Add roll joint to superstructure and arm UI
Arm UI changes:
- Update robot dimensions
- Support visualizing roll joint
- Add roll joint collision detection
Superstructure changes:
- Adding roll joint feedback loop and zeroing estimator
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I422e343890248940bba98ba3cabac94e68723a3e
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index cfc68ac..c60a2c5 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -1,37 +1,41 @@
-import numpy
+import numpy as np
-from graph_tools import *
+from y2023.control_loops.python.graph_tools import *
-neutral = to_theta_with_circular_index(-0.2, 0.33, circular_index=-1)
-zero = to_theta_with_circular_index(0.0, 0.0, circular_index=-1)
+neutral = to_theta_with_circular_index_and_roll(joint_center[0],
+ joint_center[1] + l2 - l1,
+ np.pi / 2,
+ circular_index=-1)
-neutral_to_cone_1 = to_theta_with_circular_index(0.0, 0.7, circular_index=-1)
-neutral_to_cone_2 = to_theta_with_circular_index(0.2, 0.5, circular_index=-1)
-cone_pos = to_theta_with_circular_index(1.0, 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,
+ np.pi / 2,
+ circular_index=-1)
+neutral_to_pickup_control_alpha_rolls = [(0.33, np.pi / 2), (.67, np.pi / 2)]
-neutral_to_cone_perch_pos_1 = to_theta_with_circular_index(0.4,
- 1.0,
- circular_index=-1)
-neutral_to_cone_perch_pos_2 = to_theta_with_circular_index(0.7,
- 1.5,
- circular_index=-1)
-cone_perch_pos = to_theta_with_circular_index(1.0, 2.0, 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,
+ np.pi / 2,
+ circular_index=-1)
+neutral_to_score_control_alpha_rolls = [(0.33, np.pi / 2), (.67, np.pi / 2)]
# TODO(Max): Add real paths for arm.
-points = [(neutral, "NeutralPos"), (neutral_to_cone_1, "NeutralToConePos1"),
- (neutral_to_cone_2, "NeutralToConePos2"), (cone_pos, "ConePos"),
- (neutral_to_cone_perch_pos_1, "NeutralToConePerchPos1"),
- (neutral_to_cone_perch_pos_2, "NeutralToConePerchPos2"),
- (cone_perch_pos, "ConePerchPos")]
+points = [(neutral, "NeutralPos"), (pickup_pos, "PickupPos"),
+ (score_pos, "ScorePos")]
front_points = []
back_points = []
unnamed_segments = []
named_segments = [
- ThetaSplineSegment(neutral, neutral_to_cone_1, neutral_to_cone_2, cone_pos,
- "NeutralToCone"),
- ThetaSplineSegment(neutral, neutral_to_cone_perch_pos_1,
- neutral_to_cone_perch_pos_2, cone_perch_pos,
- "NeutralToConePerch"),
+ ThetaSplineSegment("NeutralToPickup", neutral, neutral_to_pickup_1,
+ neutral_to_pickup_2, pickup_pos,
+ neutral_to_pickup_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToScore", neutral, neutral_to_score_1,
+ neutral_to_score_2, score_pos,
+ neutral_to_score_control_alpha_rolls),
]
-segments = unnamed_segments + named_segments
+segments = named_segments + unnamed_segments