FourBar_XVA.py

from sympy import pi, cos, sin, sqrt, atan, atan2, N, oo, symbols, solve
import matplotlib.pyplot as plt
import random
import time
from pprint import pprint


def getRange(arr):
    leng = sum([a[0] for a in arr])
    return -leng / 2, leng / 2


def vectTopolar(v):
    r = sqrt(v[0]**2 + v[1]**2)
    rho = atan2(v[1], v[0])
    return r, rho


def sumPos(arr, n):
    pos = [0, 0]
    for a in arr[:n]:
        pos[0] += a[0] * cos(a[1])
        pos[1] += a[0] * sin(a[1])
    return list(map(N, pos))


def sumVel(arr, n):
    pos = [0, 0]
    for a in arr[:n]:
        v = a[0] * a[2]
        pos[0] += v * cos(pi / 2 + a[1])  # cross = 90 + angel
        pos[1] += v * sin(pi / 2 + a[1])
    return list(map(N, pos))


def sumAcc(arr, n):
    pos = [0, 0]
    for a in arr[:n]:
        vn = a[0] * a[2] ** 2
        vt = a[0] * a[3]
        pos[0] += vn * cos(pi + a[1])  # negative direction
        pos[1] += vn * sin(pi + a[1])
        pos[0] += vt * cos(pi / 2 + a[1])  # cross = 90 + angel
        pos[1] += vt * sin(pi / 2 + a[1])
    return list(map(N, pos))


def solveAngel(want):
    k1 = 2 * want[2][0] * (want[0][0] * cos(want[0][1]) +
                           want[1][0] * cos(want[1][1]))
    k2 = 2 * want[2][0] * (want[0][0] * sin(want[0][1]) +
                           want[1][0] * sin(want[1][1]))
    k3 = want[0][0]**2 + want[1][0]**2 + want[2][0]**2 - want[3][0]**2 + \
         2 * want[0][0] * want[1][0] * (
         cos(want[0][1]) * cos(want[1][1]) +
         sin(want[0][1]) * sin(want[1][1]))
    A = -k1 + k3
    B = 2 * k2
    C = k1 + k3
    angel = [(-B - sqrt(B**2 - 4 * A * C)) / (2 * A),
             (-B + sqrt(B**2 - 4 * A * C)) / (2 * A)]

    ans = []
    for a in map(lambda a: 2 * atan(a), angel):
        want[2][1] = N(a)
        want[3][1] = N(pi + vectTopolar(sumPos(want, 3))[1])
        ans.append((want[2][1], want[3][1]))
    return ans


def solveOmega(want):
    x, y = symbols('x y')
    k1 = want[0][0] * cos(want[0][1]) * -want[0][2] + \
         want[1][0] * cos(want[1][1]) * -want[1][2]
    x1 = want[2][0] * cos(want[2][1])
    y1 = want[3][0] * cos(want[3][1])
    k2 = want[0][0] * sin(want[0][1]) * -want[0][2] + \
         want[1][0] * sin(want[1][1]) * -want[1][2]
    x2 = want[2][0] * sin(want[2][1])
    y2 = want[3][0] * sin(want[3][1])
    m = x1 * y2 - x2 * y1
    ans = (k1 * y2 - k2 * y1) / m, (x1 * k2 - x2 * k1) / m
    return list(map(N, ans))


def solveAlpha(want):
    x1 = want[2][0] * cos(want[2][1])
    y1 = want[3][0] * cos(want[3][1])
    k1 = want[0][0] * cos(want[0][1]) * -want[0][3] + \
         want[1][0] * cos(want[1][1]) * -want[1][3] + \
         want[0][0] * sin(want[0][1]) * want[0][2] ** 2 + \
         want[1][0] * sin(want[1][1]) * want[1][2] ** 2 + \
         want[2][0] * sin(want[2][1]) * want[2][2] ** 2 + \
         want[3][0] * sin(want[3][1]) * want[3][2] ** 2
    x2 = want[2][0] * sin(want[2][1]) * -1
    y2 = want[3][0] * sin(want[3][1]) * -1
    k2 = want[0][0] * sin(want[0][1]) * want[0][3] + \
         want[1][0] * sin(want[1][1]) * want[1][3] + \
         want[0][0] * cos(want[0][1]) * want[0][2] ** 2 + \
         want[1][0] * cos(want[1][1]) * want[1][2] ** 2 + \
         want[2][0] * cos(want[2][1]) * want[2][2] ** 2 + \
         want[3][0] * cos(want[3][1]) * want[3][2] ** 2
    m = x1 * y2 - x2 * y1
    ans = (k1 * y2 - k2 * y1) / m, (x1 * k2 - x2 * k1) / m
    return list(map(N, ans))


fig, ax = plt.subplots()
# for angle in range(0, 360, 15):
if True:
    """
    angle = 30
    want = [(152.4, pi), (50.8, angle), (177.8,), (228.6,)]
    angle = 45
    want = [(76.2, pi), (254.0, angle), (152.4,), (203.2,)]
    angle = 25
    want = [(203.2, pi), (127.0, angle), (177.8,), (152.4,)] 
    angle = 75
    want = [(203.2, pi, 0, 0), (127.0, angle, 1, 1), 
            (203.2,), (152.4,)]
    """
    angle = 85
    want = [(177.8, pi, 0, 0), (228.6, angle, 60 * 2 * pi / 60, 1),
            (76.2,), (203.2,)]

    # set axix
    want = list(map(list, want))
    want[1][1] = want[1][1] * pi / 180
    want[2] = [want[2][0], oo, oo, oo]
    want[3] = [want[3][0], oo, oo, oo]
    plt.xlim(getRange(want))
    plt.ylim(getRange(want))

    # solve
    angel = solveAngel(want)
    for a in angel:
        want[2][1] = a[0]
        want[3][1] = a[1]
        omega = solveOmega(want)
        want[2][2] = omega[0]
        want[3][2] = omega[1]
        alpha = solveAlpha(want)
        want[2][3] = alpha[0]
        want[3][3] = alpha[1]

        pprint(want)
        print("Position")
        pprint([sumPos(want, i) for i in range(4)])
        print("Velocity")
        pprint([sumVel(want, i) for i in range(4)])
        print("Acceleration")
        pprint([sumAcc(want, i) for i in range(4)])