y2023/control_loops/python/graph_paths.py

import sys

import numpy as np

from y2023.control_loops.python.graph_tools import *

named_segments = []
points = {}

points['Neutral'] = np.array((np.pi, 0.0, 0.0))

points['GroundPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
    -1.07774334, 0.40, np.pi / 2.0, circular_index=1)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToGroundPickupBackConeUp",
        start=points['Neutral'],
        control1=np.array([3.170156, -0.561227]),
        control2=np.array([2.972776, -1.026820]),
        end=points['GroundPickupBackConeUp'],
        control_alpha_rolls=[(0.30, 0.0), (.95, np.pi / 2.0)],
    ))

points['GroundPickupBackConeDownBase'] = to_theta_with_circular_index_and_roll(
    -1.11487594, 0.25, np.pi / 2.0, circular_index=1)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToGroundPickupBackConeDownBase",
        start=points['Neutral'],
        control1=np.array([3.170156, -0.561227]),
        control2=np.array([2.972776, -1.026820]),
        end=points['GroundPickupBackConeDownBase'],
        control_alpha_rolls=[(0.30, 0.0), (.95, np.pi / 2.0)],
    ))

points[
    'GroundPickupFrontConeDownBase'] = to_theta_with_circular_index_and_roll(
        0.263207, 0.24, -np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToGroundPickupFrontConeDownBase",
        start=points['Neutral'],
        control1=np.array([3.495221564200401, 0.4737763579250964]),
        control2=np.array([4.110392601248856, 1.0424853539638115]),
        end=points['GroundPickupFrontConeDownBase'],
        control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
    ))

points['ScoreFrontLowConeDownBase'] = to_theta_with_circular_index_and_roll(
    0.328533, 0.40, -np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToScoreFrontLowConeDownBase",
        start=points['Neutral'],
        control1=np.array([3.153481004695907, 0.4827717171390571]),
        control2=np.array([4.107487625131798, 0.9935705415901082]),
        end=points['ScoreFrontLowConeDownBase'],
        control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
    ))

points['ScoreFrontMidConeDownBase'] = to_theta_with_circular_index_and_roll(
    0.697179, 0.88, -np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToScoreFrontMidConeDownBase",
        start=points['Neutral'],
        control1=np.array([3.2296966803523395, 0.4274365560093907]),
        control2=np.array([3.111677631381042, 0.6783534686461494]),
        end=points['ScoreFrontMidConeDownBase'],
        control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
    ))

points['ScoreFrontHighConeDownBase'] = to_theta_with_circular_index_and_roll(
    1.04686, 1.13243, -np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToScoreFrontHighConeDownBase",
        start=points['Neutral'],
        control1=np.array([2.7653359284612185, 0.3091554519868296]),
        control2=np.array([2.6035409027556344, 0.5009078441624968]),
        end=points['ScoreFrontHighConeDownBase'],
        control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
    ))

points['GroundPickupBackCube'] = to_theta_with_circular_index_and_roll(
    -1.102, 0.30, -np.pi / 2.0, circular_index=1)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToGroundPickupBackCube",
        start=points['Neutral'],
        control1=np.array([3.153228, -0.497009]),
        control2=np.array([2.972776, -1.026820]),
        end=points['GroundPickupBackCube'],
        control_alpha_rolls=[(0.7, 0.0), (.9, -np.pi / 2.0)],
    ))

points['GroundPickupFrontCube'] = to_theta_with_circular_index_and_roll(
    0.325603, 0.255189, np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToGroundPickupFrontCube",
        start=points['Neutral'],
        control1=np.array([3.338852196583635, 0.34968650009090885]),
        control2=np.array([4.28246270189025, 1.492916470137478]),
        end=points['GroundPickupFrontCube'],
        control_alpha_rolls=[(0.4, 0.0), (.9, np.pi / 2.0)],
    ))

points['ScoreBackMidConeUp'] = to_theta_with_circular_index_and_roll(
    -1.33013, 1.08354, np.pi / 2.0, circular_index=1)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToBackMidConeUpScore",
        start=points['Neutral'],
        control1=np.array([3.6130298244820453, -0.2781204657180023]),
        control2=np.array([3.804763224169111, -0.5179424890517237]),
        end=points['ScoreBackMidConeUp'],
        control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
    ))

points['ScoreBackLowConeUp'] = to_theta_with_circular_index_and_roll(
    -1.00472, 0.672615, np.pi / 2.0, circular_index=1)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToBackLowConeUpScore",
        start=points['Neutral'],
        control1=np.array([3.260123029490386, -0.5296803702636037]),
        control2=np.array([3.1249665389044283, -0.7810758529482493]),
        end=points['ScoreBackLowConeUp'],
        control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="GroundPickupBackConeUpToBackLowConeUpScore",
        start=points['GroundPickupBackConeUp'],
        control1=np.array([2.943017165830755, -1.3740647485244808]),
        control2=np.array([2.941104610508278, -1.2434759967435083]),
        end=points['ScoreBackLowConeUp'],
        control_alpha_rolls=[],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="ScoreBackLowConeUpToScoreBackMidConeUp",
        start=points['ScoreBackLowConeUp'],
        control1=np.array([3.2930271753937728, -0.9256552441650734]),
        control2=np.array([3.6425461598470568, -0.8085366888146934]),
        end=points['ScoreBackMidConeUp'],
        control_alpha_rolls=[],
    ))

points['ScoreBackMidConeDownBase'] = to_theta_with_circular_index_and_roll(
    -1.37792406, 0.81332449, np.pi / 2.0, circular_index=1)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToMidConeDownBaseScore",
        start=points['Neutral'],
        control1=np.array([3.394572, -0.239378]),
        control2=np.array([3.654854, -0.626835]),
        end=points['ScoreBackMidConeDownBase'],
        control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
    ))

points['ScoreBackLowConeDownBase'] = to_theta_with_circular_index_and_roll(
    -1.06372, 0.442764, np.pi / 2.0, circular_index=1)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToLowConeDownBaseScore",
        start=points['Neutral'],
        control1=np.array([2.8613439132427896, -0.5868069120126034]),
        control2=np.array([2.9041434685529923, -1.240030040719494]),
        end=points['ScoreBackLowConeDownBase'],
        control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
    ))

points['HPPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
    -1.1050539, 1.34, np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToHPPickupBackConeUp",
        start=points['Neutral'],
        control1=np.array([2.0, -0.239378]),
        control2=np.array([1.6, -0.626835]),
        end=points['HPPickupBackConeUp'],
        control_alpha_rolls=[(0.7, 0.0), (.9, np.pi / 2.0)],
    ))

points['HPPickupFrontConeUp'] = np.array(
    (5.16514378449353, 1.26, -np.pi / 2.0))
#        to_theta_with_circular_index_and_roll(
#    0.265749, 1.28332, -np.pi / 2.0, circular_index=1)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToHPPickupFrontConeUp",
        start=points['Neutral'],
        control1=np.array([4.068204933788692, -0.05440997896697275]),
        control2=np.array([4.861911360838861, -0.03790410600482508]),
        end=points['HPPickupFrontConeUp'],
        control_alpha_rolls=[(0.7, 0.0), (.9, -np.pi / 2.0)],
    ))

points['ScoreFrontHighConeUp'] = to_theta_with_circular_index_and_roll(
    0.98810344, 1.37536719, -np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToFrontHighConeUpScore",
        start=points['Neutral'],
        control1=np.array([2.594244, 0.417442]),
        control2=np.array([1.51325, 0.679748]),
        end=points['ScoreFrontHighConeUp'],
        control_alpha_rolls=[(0.40, 0.0), (.95, -np.pi / 2.0)],
    ))

points['ScoreFrontMidConeUp'] = to_theta_with_circular_index_and_roll(
    0.43740453, 1.06330555, -np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToFrontMidConeUpScore",
        start=points['Neutral'],
        control1=np.array([3.0, 0.317442]),
        control2=np.array([2.9, 0.479748]),
        end=points['ScoreFrontMidConeUp'],
        control_alpha_rolls=[(0.40, 0.0), (.95, -np.pi / 2.0)],
    ))

points['ScoreFrontLowCube'] = to_theta_with_circular_index_and_roll(
    0.325603, 0.30, np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToScoreFrontLowCube",
        start=points['Neutral'],
        control1=np.array([3.338852196583635, 0.34968650009090885]),
        control2=np.array([4.28246270189025, 1.492916470137478]),
        end=points['ScoreFrontLowCube'],
        control_alpha_rolls=[(0.4, 0.0), (.9, np.pi / 2.0)],
    ))

points['ScoreFrontMidCube'] = to_theta_with_circular_index_and_roll(
    0.517846, 0.87, np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToScoreFrontMidCube",
        start=points["Neutral"],
        control1=np.array([3.1310824883477952, 0.23591705727105095]),
        control2=np.array([3.0320025094685965, 0.43674789928668933]),
        end=points["ScoreFrontMidCube"],
        control_alpha_rolls=[(0.4, np.pi * 0.0), (0.95, np.pi * 0.5)],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="ScoreFrontLowCubeToScoreFrontMidCube",
        start=points["ScoreFrontLowCube"],
        control1=np.array([3.8237323383577078, 1.2979562720646056]),
        control2=np.array([3.63484177908944, 1.008850428344438]),
        end=points["ScoreFrontMidCube"],
        control_alpha_rolls=[],
    ))

points['ScoreFrontHighCube'] = to_theta_with_circular_index_and_roll(
    0.901437, 1.16, np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToScoreFrontHighCube",
        start=points["Neutral"],
        control1=np.array([2.537484161662287, 0.059700523547219]),
        control2=np.array([2.449391812539668, 0.4141564369176016]),
        end=points["ScoreFrontHighCube"],
        control_alpha_rolls=[(0.4, np.pi * 0.0), (0.95, np.pi * 0.5)],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="ScoreFrontMidCubeToScoreFrontHighCube",
        start=points["ScoreFrontMidCube"],
        control1=np.array([2.9229652375897004, 0.7771801809056819]),
        control2=np.array([2.634276444896239, 0.5696525540129302]),
        end=points["ScoreFrontHighCube"],
        control_alpha_rolls=[],
    ))

points['ScoreBackLowCube'] = to_theta_with_circular_index_and_roll(
    -1.102, 0.3212121, -np.pi / 2.0, circular_index=1)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToScoreLowBackCube",
        start=points['Neutral'],
        control1=np.array([3.153228, -0.497009]),
        control2=np.array([2.972776, -1.026820]),
        end=points['ScoreBackLowCube'],
        control_alpha_rolls=[(0.7, 0.0), (.9, -np.pi / 2.0)],
    ))

points['ScoreBackMidCube'] = to_theta_with_circular_index_and_roll(
    -1.27896, 0.84, -np.pi / 2.0, circular_index=1)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToScoreBackMidCube",
        start=points["Neutral"],
        control1=np.array([3.3485646154655404, -0.4369603013926491]),
        control2=np.array([3.2653593368256995, -0.789587049476034]),
        end=points["ScoreBackMidCube"],
        control_alpha_rolls=[(0.3, -np.pi * 0.0), (0.95, -np.pi * 0.5)],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="ScoreBackLowCubeToScoreBackMidCube",
        start=points["ScoreBackLowCube"],
        control1=np.array([3.1075630474968694, -1.1675095818664531]),
        control2=np.array([3.3377520447373232, -1.1054408842366303]),
        end=points["ScoreBackMidCube"],
        control_alpha_rolls=[],
    ))

# TODO(austin): This doesn't produce the next line...
#points['ScoreBackHighCube'] = to_theta_with_circular_index_and_roll(
#    -1.60932, 1.16839, np.pi / 2.0, circular_index=0)
points['ScoreBackHighCube'] = np.array(
    (4.77284735761704, -1.19952193130714, -np.pi / 2.0))

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToScoreBackHighCube",
        start=points["Neutral"],
        control1=np.array([3.6804854484103684, -0.3494541095053125]),
        control2=np.array([3.9889380578509517, -0.6637934755748516]),
        end=points["ScoreBackHighCube"],
        control_alpha_rolls=[(0.3, -np.pi * 0.0), (0.95, -np.pi * 0.5)],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="ScoreBackMidCubeToScoreBackHighCube",
        start=points["ScoreBackMidCube"],
        control1=np.array([4.03651864313893, -0.919229198708873]),
        control2=np.array([4.377346803653962, -1.0167608157302999]),
        end=points["ScoreBackHighCube"],
        control_alpha_rolls=[],
    ))

points['GroundPickupFrontConeUp'] = to_theta_with_circular_index_and_roll(
    0.313099, 0.380, -np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToGroundPickupFrontConeUp",
        start=points['Neutral'],
        control1=np.array([3.153481004695907, 0.4827717171390571]),
        control2=np.array([4.107487625131798, 0.9935705415901082]),
        end=points['GroundPickupFrontConeUp'],
        control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
    ))

points['ScoreFrontLowConeUp'] = to_theta_with_circular_index_and_roll(
    0.349687, 0.468804, -np.pi / 2.0, circular_index=0)

named_segments.append(
    ThetaSplineSegment(
        name="NeutralToScoreFrontLowConeUp",
        start=points['Neutral'],
        control1=np.array([3.153481004695907, 0.4827717171390571]),
        control2=np.array([4.107487625131798, 0.9935705415901082]),
        end=points['ScoreFrontLowConeUp'],
        control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="GroundPickupFrontConeUpToScoreFrontLowConeUp",
        start=points['GroundPickupFrontConeUp'],
        control1=np.array([4.14454438793702, 1.680256664914554]),
        control2=np.array([4.159014136030164, 1.6617266432775355]),
        end=points['ScoreFrontLowConeUp'],
        control_alpha_rolls=[],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="ScoreFrontLowConeUpToScoreFrontMidConeUp",
        start=points['ScoreFrontLowConeUp'],
        control1=np.array([4.144103145250675, 1.3519566301042056]),
        control2=np.array([3.5357641970552223, 0.8105698293886593]),
        end=points['ScoreFrontMidConeUp'],
        control_alpha_rolls=[],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="ScoreFrontMidConeUpToScoreFrontHighConeUp",
        start=points['ScoreFrontMidConeUp'],
        control1=np.array([2.417981958011055, 0.48234108399079134]),
        control2=np.array([2.1651435746478045, 0.4937628492739232]),
        end=points['ScoreFrontHighConeUp'],
        control_alpha_rolls=[],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="GroundPickupBackConeUpToGroundPickupBackConeDownBase",
        start=points["GroundPickupBackConeUp"],
        control1=np.array([2.984750907058771, -1.4175755629898785]),
        control2=np.array([2.9834020302847164, -1.4959006770007095]),
        end=points["GroundPickupBackConeDownBase"],
        control_alpha_rolls=[],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="GroundPickupBackCubeToGroundPickupBackConeUp",
        start=points["GroundPickupBackCube"],
        control1=np.array([2.9814712671305497, -1.4752615794585657]),
        control2=np.array([2.9814712671305497, -1.4752615794585657]),
        end=points["GroundPickupBackConeUp"],
        control_alpha_rolls=[],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="GroundPickupBackCubeToGroundPickupBackConeDownBase",
        start=points["GroundPickupBackCube"],
        control1=np.array([2.9915062872070943, -1.5453319249912183]),
        control2=np.array([3.0113316601735853, -1.5625220857410058]),
        end=points["GroundPickupBackConeDownBase"],
        control_alpha_rolls=[],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="GroundPickupFrontConeUpToGroundPickupFrontConeDownBase",
        start=points["GroundPickupFrontConeUp"],
        control1=np.array([4.178303420953318, 1.7954892324947347]),
        control2=np.array([4.198694185882847, 1.8611851211658763]),
        end=points["GroundPickupFrontConeDownBase"],
        control_alpha_rolls=[],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="GroundPickupFrontCubeToGroundPickupFrontConeUp",
        start=points["GroundPickupFrontCube"],
        control1=np.array([4.152678427672294, 1.755703782155648]),
        control2=np.array([4.115445030197163, 1.77599054062196]),
        end=points["GroundPickupFrontConeUp"],
        control_alpha_rolls=[],
    ))

named_segments.append(
    ThetaSplineSegment(
        name="GroundPickupFrontCubeToGroundPickFrontCubeDownBase",
        start=points["GroundPickupFrontCube"],
        control1=np.array([4.126486425254001, 1.838621758570565]),
        control2=np.array([4.1585708953556, 1.8633805468551703]),
        end=points["GroundPickupFrontConeDownBase"],
        control_alpha_rolls=[],
    ))

front_points = []
back_points = []
unnamed_segments = []
segments = named_segments + unnamed_segments

# This checks that all points are unique

seen_segments = []

for segment in segments:
    # check for equality of the start and end values

    if (segment.start.tolist(), segment.end.tolist()) in seen_segments:
        print("Repeated value")
        segment.Print(points)
        sys.exit(1)
    else:
        seen_segments.append((segment.start.tolist(), segment.end.tolist()))

seen_points = []

for point in points:
    if point in seen_points:
        print(f"Repeated value {point}")
        sys.exit(1)
    else:
        seen_points.append(point)