dynamic

from sympy import *
import sympy as sy
import math
import numpy as np
import matplotlib.pyplot as plt

r1 = 80
r2 = 50
r3 = 40
r4 = 8
fi = math.pi/4
theta45 = 2*math.pi/3
theta23max = math.pi/36
theta23min = -31*math.pi/36
theta34max = -math.pi/9
theta34min = -13*math.pi/18
a = r2
b = r3
all2 = []
all3 = []
all4 = []
all5 = []
for theta4 in np.arange(0,2*math.pi,math.pi/180):
    c = r1 - r4*sy.cos(theta4)
    d = -r4*sy.sin(theta4)
    theta2 = Symbol('theta2')
    theta2 = solve(c*sy.cos(theta2)+d*sy.cos(theta2)-(a**2+c**2+d**2-b**2)/(2*a),theta2)
    for i in range(0,len(theta2)):
        try:
            if theta2[i] < 2*math.pi:
                pass
        except TypeError:
            continue
        if theta2[i] < 2*math.pi:
            theta3_1 = Symbol('theta3_1')
            theta3_2 = Symbol('theta3_2')
            theta3_1 = solve(sy.cos(theta3_1)-((c-a*sy.cos(theta2[i]))/b),theta3_1)
            theta3_2 = solve(sy.sin(theta3_2)-((d-a*sy.sin(theta2[i]))/b),theta3_2)
            premin = 10000
            for k in theta3_1:
                for j in theta3_2:
                    value = abs(j-k)
                    try:
                        if value < premin:
                            pass
                    except TypeError:
                        continue
                    if value < premin:
                        premin = value
                        theta3 = (k + j)/2
        else:
            continue
        theta5 = theta45 + theta4
        if theta5 > 2*math.pi:
            theta5 = theta5 - 2*math.pi
        all5.append(theta5)
        all2.append(theta2[i])
        all3.append(theta3)
        all4.append(theta4)

fig = plt.figure()
ax1 = fig.add_subplot(111)
ax1.set_title('θ3-θ5')
plt.xlabel('θ5')
plt.ylabel('θ3')
ax1.scatter(all3,all5,c = 'r',marker = 'o')
plt.legend('x1')
plt.show()