Позиционный анализ системы

Question

Я пытаюсь выполнить этот анализ кинематической позиции в Python с использованием библиотеки Numpy. Тем не менее, сталкиваются с некоторыми проблемами по поводу кода нижеАнализ позиции xC и xP неверен.

import numpy as np
import math as mt
import matplotlib.pyplot as plt


#Linkage Dimensions in meter
a = 0.130  #Crank Lenght
b = 0.200  #Coupler lenght
c = 0.170  #Rocker Lenght
d = 0.220  #Lenght between ground pins
p = 0.250  #Lenght from B to P
gamma = 20*mt.pi/180  #Angle between BP and coupler converted radian

print('Math module Pi',mt.pi)

#Coordinates of ground pins
x0 = np.array([[0],[0]])  #point A the origin of system
xD = np.array([[d],[0]])  #point D

N=360 #Number of times perform position analysis 

Эта матрица создания для позиции:

#Allocate for position B,C and P
xB = np.zeros([2,N])
xC = np.zeros([2,N])
xP = np.zeros([2,N])

#Allocate space for angles
theta2 = np.zeros([1,N])
theta3 = np.zeros([1,N])
theta4 = np.zeros([1,N])



#Defining Unit Vector Function
#Calculates the unit vector and unit normal for a given angle
#theta = angle of unitvector
#e     = unit vector in the direction of theta
#n     = unit normal to the vector e

def UnitVector(theta):
    e = np.array([[np.cos(theta)], [np.sin(theta)]])
    n = np.array([[-np.sin(theta)], [np.cos(theta)]])
    return [e,n]


#Defining FindPos function
#Calculate the position of a point on a link using relative position
#formula
#x0 = position of first point on the link
#L  = Lenght of vector between first and second points
#e  = Unit Vector between first and second points
#x  = Position of second point on the link

def FindPos(x0,L,e):
    x = x0 + L + e
    return x

t3 = float((mt.pi)/4)   #Initial Guess for Newton-Raphson
t4 = float((mt.pi)/2)   #Initial Guess for Newton-Raphson

print('Initial Guess for t3: ',t3)
print('Initial Guess for t4: ',t4)

В приведенном ниже блоке описывается двухмерный алгоритм Ньютона-Рафсона:

for i in range(1,N):

    theta2[0,(i-1)] = (i-1)*(2*mt.pi)/(N-1)

    #Newton-Raphson Calculation
    for j in range(1,6):
        phi11 = a*mt.cos(theta2[0,(i-1)]) + b*mt.cos(t3) - c*mt.cos(t4) - d
        phi12 = a*mt.sin(theta2[0,(i-1)]) + b*mt.sin(t3) - c*mt.sin(t4)

        phi = np.array([[phi11], [phi12]])

        norm = ((phi[0])**2 + (phi[1])**2)**(0.5)

        #If constrain equations are satisfied then terminate 
        if (norm < 0.000000000001):

            theta3[0,i-1] = t3
            theta4[0,i-1] = t4

            break

Расчет матрицы Якобиана:

        #Calculate Jacobien Matrix
        J00 = -b*mt.sin(t3)   #(1,1) element of matrix
        J01 = c*mt.sin(t4)    #(1,2) eleemnt of matrix
        J10 = b*mt.cos(t3)    #(2,1) element of matrşx
        J11 = -c*mt.cos(t4)   #(2,2) element of mattix

        J = np.array([[J00, J01],[J10, J11]])

        #Update Variables using Newton-Raphson Equation
        dq = np.linalg.solve(J,phi)

        t3 = t3 + dq[0,0]
        t4 = t4 + dq[1,0]

    #Calculating UnitVectors
    [e2,n2]   = UnitVector(theta2[0,(i-1)])
    [e3,n3]   = UnitVector(theta3[0,(i-1)])
    [e4,n4]   = UnitVector(theta4[0,(i-1)])
    [eBP,nBP] = UnitVector(theta3[0,(i-1)]+gamma)

Замена вычисленных значений на нули в матрице:

    #Solve for position of posints B, C and P on the linkage
    xB[:,(i-1)][0]   = FindPos(x0,a,e2)[0][0]
    xB[:,(i-1)][1]   = FindPos(x0,a,e2)[1][0]

Построение этих значений в виде позиции:

plt.plot(xB[0,:359],xB[1,:359])
plt.xlabel('x position [m]')
plt.ylabel('y position [m]')
plt.title('Four Bar Crank Position Analysis')
plt.grid(True)
plt.show()

Здесь появляются неправильные результаты. Графики xC и xP неверны.

xC[:,(i-1)][0]   = FindPos(xD,c,e4)[0][0]
xC[:,(i-1)][1]   = FindPos(xD,c,e4)[1][0]
xP[:,(i-1)][0]   = FindPos(xB[:,i-1],p,eBP)[0][0]
xP[:,(i-1)][1]   = FindPos(xB[:,i-1],p,eBP)[1][0]

plt.plot(xC[0,:359],xC[1,:359])
plt.xlabel('x position [m]')
plt.ylabel('y position [m]')
plt.title('Four Bar Crank Position Analysis')
plt.grid(True)
plt.show()

plt.plot(xP[0,:359],xP[1,:359])
plt.xlabel('x position [m]')
plt.ylabel('y position [m]')
plt.title('Four Bar Crank Position Analysis')
plt.grid(True)
plt.show()

Любые предложения приветствуются. Благодарю.