link가 3개인 로봇을 3차원 입체공간에 그려 보겠습니다.
보통 첫번째 link는 z방향(바닥에서 수직)이어서 비슷한 형상으로 예시했습니다.
결과물 부터 보겠습니다.
프로그램을 단순화하기 위해 xlim, xticks 등으로 그래프 크기를 보기 좋게 하지는 않았습니다.
기구학적 구현 원리를 파악하는 용도에 집중한 것이니 소스랑 비교해 보시기 바랍니다.
# 3link 3d forward kinematics
# 정기구학 3link 로봇을 입체 그래프로 나타내 보는 작업입니다.
# coding을 하는 방법은 매우 다양하니 참고만 하세요
import numpy as np # numpy는 삼각함수 행렬 계산에 필수입니다
import matplotlib.pyplot as plt # matplotlib은 계산 결과를 그래프(그림)으로 표현하는 라이브러리로
# MATLAB의 기본적인 기능은 수행 가능합니다
plt.figure(figsize=(7, 7)) # 가로,세로 크기를 지정합니다. 지정하지 않으면 알아서 해 줍니다.
plt.subplot(projection='3d') # 입체형 그래프를 그리라는 지정
# initial value
p0x = 0; p0y = 0; p0z = 0 # 원점을 x,y,z축 각각 0으로 설정
l1 = 0.5; l2 = 1.5; l3 = 2 # link 길이
th1 = 45 * np.pi / 180 # degree 45도를 radian으로 변경
th2 = 20 * np.pi / 180
th3 = 30 * np.pi / 180
# forward kinematics
p1x = p0x
p1y = p0y
p1z = l1
p2x = (l2 * np.cos(th2)) * np.cos(th1)
p2y = (l2 * np.cos(th2)) * np.sin(th1)
p2z = l1 + l2 * np.sin(th2)
p3x = (l1 * np.cos(th2) + l3 * np.cos(th2 + th2)) * np.cos(th1)
p3y = (l1 * np.cos(th2) + l3 * np.cos(th2 + th2)) * np.sin(th1)
p3z = l1 + l2 * np.sin(th2) + l3 * np.sin(th2 + th3)
plt.plot([p0x, p1x, p2x, p3x], [p0y, p1y, p2y, p3y], [p0z, p1z, p2z, p3z]) # link 그려주기
plt.plot(p3x, p3y, p3z, '*r') # link 끝에 붉은색 *표 하기
plt.title('3link robot') # 그래프에 제목
plt.xlabel('X axis') # x축에 표현해줄 문자
plt.ylabel('Y axis') # y축에 표현해줄 문자
plt.show() # 이 문장이 있어야 화면에 그래프를 보여줍니다