y2023/control_loops/python/graph_paths.py

import numpy

from 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_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_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)

# 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")]
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"),
]

segments = unnamed_segments + named_segments