import kinematics as kin
import DataTypes as dt

init_deg: dt.DegArray = dt.DegArray(
    [
        [90, 45, 140],
        [90, 45, 140],
        [90, 45, 140],
        [90, 135, 40],
        [90, 135, 40],
        [90, 135, 40],
    ]
)

init90_deg: dt.DegArray = dt.DegArray(
    [
        [90, 90, 90],
        [90, 90, 90],
        [90, 90, 90],
        [90, 90, 90],
        [90, 90, 90],
        [90, 90, 90],
    ]
)

init_pos: dt.RadArray = init_deg.to_rad()
center_points: dt.PosArray = kin.ikpyForward(init_pos)