y2023/control_loops/python/graph_paths.py

import numpy as np

from y2023.control_loops.python.graph_tools import *

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(
    -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(
    -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.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.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])
neutral_to_hp_pickup_back_cone_up_2 = np.array([1.6, -0.626835])
neutral_to_hp_pickup_back_cone_up_alpha_rolls = [
    (0.7, 0.0),
    (.9, np.pi / 2.0),
]
hp_pickup_back_cone_up = to_theta_with_circular_index_and_roll(
    -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.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.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])
cone_down_pos = to_theta_with_circular_index_and_roll(0.7,
                                                      0.11,
                                                      np.pi / 2.0,
                                                      circular_index=0)

neutral_to_cube_1 = np.array([2.396694, 0.508020])
neutral_to_cube_2 = np.array([2.874513, 0.933160])

cube_pos = to_theta_with_circular_index_and_roll(0.7,
                                                 0.24,
                                                 np.pi / 2.0,
                                                 circular_index=0)

neutral_to_pickup_control_alpha_rolls = [
    (0.30, 0.0),
    (.95, np.pi / 2.0),
]

neutral_to_score_1 = np.array([0.994244, -1.417442])
neutral_to_score_2 = np.array([1.711325, -0.679748])

score_low_pos = to_theta_with_circular_index_and_roll(-(0.41 / 2 + 0.49),
                                                      0 + 0.05,
                                                      np.pi / 2.0,
                                                      circular_index=1)

neutral_to_score_low_2 = np.array([3.37926599, -0.73664663])

score_mid_cube_pos = to_theta_with_circular_index_and_roll(-(0.58 + 0.49),
                                                           0.6 + 0.05,
                                                           np.pi / 2.0,
                                                           circular_index=0)

score_high_cone_pos = to_theta_with_circular_index_and_roll(-1.01,
                                                            1.17 + 0.05,
                                                            np.pi / 2.0,
                                                            circular_index=0)

score_high_cube_pos = to_theta_with_circular_index_and_roll(-1.01,
                                                            0.90 + 0.05,
                                                            np.pi / 2.0,
                                                            circular_index=0)

neutral_to_back_score_control_alpha_rolls = [(0.40, 0.0), (.95, np.pi / 2.0)]
neutral_to_front_score_control_alpha_rolls = [(0.40, 0.0), (.95, -np.pi / 2.0)]

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"),
          (score_front_high_cone_up_pos, "ScoreFrontHighConeUpPos"),
          (score_front_mid_cone_up_pos, "ScoreFrontMidConeUpPos"),
          (score_mid_cone_down_pos, "ScoreBackMidConeDownPos"),
          (score_mid_cube_pos, "ScoreMidCubePos"),
          (score_high_cone_pos, "ScoreHighConePos"),
          (score_high_cube_pos, "ScoreHighCubePos"), (cube_pos, "CubePos")]
front_points = []
back_points = []
unnamed_segments = []
named_segments = [
    ThetaSplineSegment("NeutralToGroundPickupBackConeUp", neutral,
                       neutral_to_cone_up_1, neutral_to_cone_up_2,
                       ground_pickup_back_cone_up,
                       neutral_to_pickup_control_alpha_rolls),
    ThetaSplineSegment("NeutralToGroundPickupBackConeDown", neutral,
                       neutral_to_ground_pickup_back_cone_down_1,
                       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,
                       hp_pickup_back_cone_up,
                       neutral_to_hp_pickup_back_cone_up_alpha_rolls),
    ThetaSplineSegment("NeutralToFrontHighConeUpScore", neutral,
                       neutral_to_score_front_high_cone_up_1,
                       neutral_to_score_front_high_cone_up_2,
                       score_front_high_cone_up_pos,
                       neutral_to_front_score_control_alpha_rolls),
    ThetaSplineSegment("NeutralToFrontMidConeUpScore", neutral,
                       neutral_to_score_front_mid_cone_up_1,
                       neutral_to_score_front_mid_cone_up_2,
                       score_front_mid_cone_up_pos,
                       neutral_to_front_score_control_alpha_rolls),
    ThetaSplineSegment("NeutralToConeDown", neutral, neutral_to_cone_down_1,
                       neutral_to_cone_down_2, cone_down_pos,
                       neutral_to_pickup_control_alpha_rolls),
    ThetaSplineSegment("NeutralToCube", neutral, neutral_to_cube_1,
                       neutral_to_cube_2, cube_pos,
                       neutral_to_pickup_control_alpha_rolls),
    ThetaSplineSegment("NeutralToLowScore", neutral, neutral_to_score_1,
                       neutral_to_score_low_2, score_low_pos,
                       neutral_to_back_score_control_alpha_rolls),
    ThetaSplineSegment("NeutralToBackMidConeUpScore", neutral,
                       neutral_to_score_back_mid_cone_up_1,
                       neutral_to_score_back_mid_cone_up_2,
                       score_back_mid_cone_up_pos,
                       neutral_to_back_score_control_alpha_rolls),
    ThetaSplineSegment("NeutralToMidConeDownScore", neutral,
                       neutral_to_score_mid_cone_down_1,
                       neutral_to_score_mid_cone_down_2,
                       score_mid_cone_down_pos,
                       neutral_to_back_score_control_alpha_rolls),
    ThetaSplineSegment("NeutralToMidCubeScore", neutral, neutral_to_score_1,
                       neutral_to_score_2, score_mid_cube_pos,
                       neutral_to_back_score_control_alpha_rolls),
    ThetaSplineSegment("NeutralToHighConeScore", neutral, neutral_to_score_1,
                       neutral_to_score_2, score_high_cone_pos,
                       neutral_to_back_score_control_alpha_rolls),
    ThetaSplineSegment("NeutralToHighCubeScore", neutral, neutral_to_score_1,
                       neutral_to_score_2, score_high_cube_pos,
                       neutral_to_back_score_control_alpha_rolls),
]

segments = named_segments + unnamed_segments