[기구학] 4절 링크 위치해석 (position analysis) (2) 파이썬으로 위치계산기 만들기

지난 글에서 유도한 $\theta_{3}$ 와 $\theta_{4}$ 공식을 파이썬으로 코딩하였습니다. known 에 각 링크 길이와 $\theta_{1}$을 넣어주면 됩니다. Norton 기구학 책의 4-1 문제를 풀어보았습니다. 

 

import numpy as np

#1) known
L_1=100
L_2=40
L_3=120
L_4=80
theta_2=40*np.pi/180

#2) theta-3 calculation

A=(L_4**2-L_2**2-L_3**2-L_1**2)/(2*L_2*L_3)
B=L_1/L_3
C=L_1/L_2
a=A+B*np.cos(theta_2)+np.cos(theta_2)-C
b=-2*np.sin(theta_2)
c=A+B*np.cos(theta_2)-np.cos(theta_2)+C

theta_3_1=2*np.arctan( (-b+np.sqrt(b**2-4*a*c))/(2*a)  )
theta_3_2=2*np.arctan( (-b-np.sqrt(b**2-4*a*c))/(2*a)  )

#3) theta-4 calculation

E=(L_3**2-L_2**2-L_4**2-L_1**2)/(-2*L_2*L_4)
F=L_1/L_4
G=L_1/L_2

e=E-F*np.cos(theta_2)+np.cos(theta_2)-G
f=-2*np.sin(theta_2)
g=E-F*np.cos(theta_2)-np.cos(theta_2)+G

theta_4_1=2*np.arctan( (-f+np.sqrt(f**2-4*e*g))/(2*e)  )
theta_4_2=2*np.arctan( (-f-np.sqrt(f**2-4*e*g))/(2*e)  )

#4) print

print("theta3_open : "+str(theta_3_2*180/np.pi))
print("theta3_crossed : "+str(theta_3_1*180/np.pi))
print("theta4_open : "+str(theta_4_2*180/np.pi))
print("theta4_crossed : "+str(theta_4_1*180/np.pi))

 

아래는 결과입니다. 

 

>>> print("theta3_open : "+str(theta_3_2*180/np.pi))
theta3_open : 20.297882788196485
>>> print("theta3_crossed : "+str(theta_3_1*180/np.pi))
theta3_crossed : -60.97796679586445
>>> print("theta4_open : "+str(theta_4_2*180/np.pi))
theta4_open : 57.324880070360805
>>> print("theta4_crossed : "+str(theta_4_1*180/np.pi))
theta4_crossed : -98.00496407802875

 

예제 결과와 잘 일치합니다.