Python으로 학습해본 Craig의 Robotics 2장 예제

아마 로보틱스를 학습하시는 분이라면 Craig라는 이름이 반가우실 겁니다.^^ 아마 가장 많이 사용하는 로보틱스 교재일 거라 예상하는데요. 오늘은 그 책의 2장에서 다루는 일부 예제를 Python으로 직접 그려보는 시간을 가져볼까합니다.^^ Craig 책 2장의 이론적인 부분은 [바로가기]에서 정리를 했었구요... 그 후 Python에서 로보틱스적인 뭔가를 그리기 위한 기초 함수를 몇 개 간신히(^^) 만들었었습니다. [바로가기]에서 만든 좌표계나 벡터를 그리는 함수를 이용해서 오늘의 글을 진행할려구요^^. 오늘의 글에서 다루는 예제와 로보틱스 함수들은 모두 GitHub에 아무도 보지 않지만~~~^^ 공개되어 있답니다.

GItHub에 drawRobotics 바로가기

아무튼 예제를 보죠.. Craig 책 2-1예제입니다.^^

간단하게 Z축 중심으로 회전하는 예제입니다. 뭐 풀이는 다 나와있고. 오늘의 목적은 예제의 그림을 3D 공간에서 Python을 이용해서 그리는 거니까요^^

# Introduction to Robotics 3rd Edition by Craig
# Example 2-1.
# Figure 2.6 shows a frame {B} that is rotated relative to frame {A} about Z by 30 degrees.

import matplotlib.pyplot as plt
import drawRobotics as dR
import numpy as np

P_atB = np.array([0,2,0])

Rot_AtoB = dR.RotZ(dR.conv2Rad(30))

P_atA = np.dot(Rot_AtoB, P_atB)

print('P_atB = ', P_atB)
print('Rot_AtoB = ', Rot_AtoB)
print('P_atA = Rot_AtoB * P_atA = ', P_atA)

AORG = np.array([0,0,0])
hat_X_atA = np.array([1,0,0])
hat_Y_atA = np.array([0,1,0])
hat_Z_atA = np.array([0,0,1])

BORG = np.dot(Rot_AtoB, AORG)
hat_X_atB = np.dot(Rot_AtoB, hat_X_atA)
hat_Y_atB = np.dot(Rot_AtoB, hat_Y_atA)
hat_Z_atB = np.dot(Rot_AtoB, hat_Z_atA)

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

dR.drawPointWithAxis(ax, AORG, hat_X_atA, hat_Y_atA, hat_Z_atA, pointEnable=False)
dR.drawPointWithAxis(ax, BORG, hat_X_atB, hat_Y_atB, hat_Z_atB, pointEnable=False, lineStyle='--')
dR.drawVector(ax, AORG, P_atA, arrowstyle='-|>', annotationString=' $ ^{A}P $ ')

ax.set_xlim([-1.5,1.5]), ax.set_ylim([-0.1,2]), ax.set_zlim([-0.1,1.5])
ax.set_xlabel('X axis'), ax.set_ylabel('Y axis'), ax.set_zlabel('Z axis')
ax.view_init(azim=-90, elev=90)
plt.show()

일단 drawRobotics라는 모듈을 import하고나면 그리는 것은 어렵지 않습니다. 당연하지만, P_atB와 P_atA를 계산하고 나면, fig = 안에서 A좌표계를 그리고, B좌표계를 그리고, 다음 P_atA 벡터를 그린겁니다.^^

중간에 pinrt문이 있지만, 결과를 출력해서 교재와 차이가 없다는 것을 확인했구요

그리고 그림이 출력되었네요. view 각도를 XY평면을 보도록 조절했지만, python matplotlib는 저렇게 표현이 되나봅니다. detail에는 또 좀 약함.ㅠㅠ.

그 다음은 예제 2-2입니다. 이번에는 Z축 회전후에 X, Y축으로 이동한 것인데요...

# Introduction to Robotics 3rd Edition by Craig
# Example 2-2.
# Figure 2.8 shows a frame {B}, which is rotated relative to frame {A} about hat_Z by 30 degrees, 
# translated 10 units in hat_X_A, and translated 5 units in hat_Y_A. Find P_atA, where, P_atB = [3, 7, 0].

import matplotlib.pyplot as plt
import drawRobotics as dR
import numpy as np

P_atB = np.array([3, 7, 0, 1])

RotZ = dR.RotZ(dR.conv2Rad(30))
Dq = np.array([10,5,0])

AORG = np.array([ [1,0,0,0],
					[0,1,0,0],
					[0,0,1,0],
					[0,0,0,1]])
def_Bframe = np.r_[np.c_[RotZ, Dq], [np.array([0,0,0,1])]]

P_atA = np.dot(def_Bframe, P_atB)

print('def_Bframe = ', def_Bframe)
print('P_atA = ', P_atA)

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

dR.drawPointWithAxis(ax, def_Bframe, vectorLength=3)
dR.drawPointWithAxis(ax, AORG, pointEnable=False, vectorLength=3)

dR.drawVector(ax, np.array([0,0,0]), P_atA, arrowstyle='-|>', annotationString = ' $ ^{A}P $ ')
dR.drawVector(ax, Dq, P_atA, arrowstyle='-|>', lineColor='c', proj=False, lineStyle='--')
dR.drawVector(ax, np.array([0,0,0]), Dq, arrowstyle='-|>', lineColor='c', 
				proj=False, lineStyle='--', annotationString = 'Dq')

dR.drawPointWithAxis(ax, Dq, np.array([1,0,0]), np.array([0,1,0]), np.array([0,0,1]), 
						pointEnable=False, lineStyle='--', vectorLength=3)

ax.set_xlim([-2.5,13.5]), ax.set_ylim([-1.5,14]), ax.set_zlim([-0.1,5])
ax.set_xlabel('X axis'), ax.set_ylabel('Y axis'), ax.set_zlabel('Z axis')
ax.view_init(azim=-90, elev=90)
plt.show()

위 코드라고 2-1예제와 다를 건 없습니다.^^. 근데 제가 사용하는 code highlighter는 들여쓰기를 꽤 길게 하네요..ㅠㅠ. 전 4칸으로 설정했는데ㅠㅠ.

아무튼.. 결과는 위와 같이 잘 출력되고...

그걸 그림으로 그려보니 위와 같습니다.^^

이번에는 예제 2-3입니다.

# Introduction to Robotics 3rd Edition by Craig
# Example 2-3.
# There is a vector P1_atA. 
# We wish to compute the vector obtained by rotating this vector about hat_Z by 30 degrees.
# Call the new vector P2_atA.

import matplotlib.pyplot as plt
import drawRobotics as dR
import numpy as np

P1_atA = np.array([0, 2, 0])

AORG = np.array([0, 0, 0])
hat_X_A, hat_Y_A, hat_Z_A = np.array([1,0,0]), np.array([0,1,0]), np.array([0,0,1])

RotZ = dR.RotZ(dR.conv2Rad(30))

P2_atA = np.dot(RotZ, P1_atA)

print('P1_atA = ', P1_atA)
print('RotZ = ', RotZ)
print('P2_atA = ', P2_atA)

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

dR.drawPointWithAxis(ax, AORG, hat_X_A, hat_Y_A, hat_Z_A, pointEnable=False)

dR.drawVector(ax, AORG, P2_atA, arrowstyle='-|>', proj=False, annotationString=' $ ^{A}P_{2} $ ')
dR.drawVector(ax, AORG, P1_atA, arrowstyle='-|>', lineColor='c', proj=False, lineStyle='--', 
																annotationString=' $ ^{A}P_{1} $ ')

ax.set_xlim([-2,2]), ax.set_ylim([-1,3]), ax.set_zlim([-0.1,5])
ax.set_xlabel('X axis'), ax.set_ylabel('Y axis'), ax.set_zlabel('Z axis')
ax.view_init(azim=-90, elev=90)
plt.show()

위에서는 좌표계 하나를 그리고, P1과 P2를 그렸는데요. 이때 P1은 점선으로 처리했습니다.

그러면 위 그림처럼 나타나네요^^

이번에는 예제 2-4인데요.

# Introduction to Robotics 3rd Edition by Craig
# Example 2-4.
# P1_atA. We wish rotate it about hat_Z by 30 degrees and 
# translate it 10 units in hat_X_A, and 5 units in hat_Y_A.
# Find P2_atA, where P1_atA = [3, 7, 0].

import matplotlib.pyplot as plt
import drawRobotics as dR
import numpy as np

P1_atA = np.array([3, 7, 0, 1])

Dq = np.array([10,5,0])

operatorT = np.r_[np.c_[dR.RotZ(dR.conv2Rad(30)), Dq], [np.array([0,0,0,1])] ]

P2_atA = np.dot(operatorT, P1_atA)

AORG = np.array([0,0,0])
hat_X_A, hat_Y_A, hat_Z_A = np.array([1,0,0]), np.array([0,1,0]), np.array([0,0,1])

print('P1_atA = ', P1_atA)
print('Dq = ', Dq)
print('operatorT = ', operatorT)
print('P2_atA = ', P2_atA)

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

dR.drawPointWithAxis(ax, AORG, hat_X_A, hat_Y_A, hat_Z_A, pointEnable=False, vectorLength=3)

dR.drawVector(ax, AORG, P1_atA, arrowstyle='-|>', proj=False, annotationString=' $ ^{A}P_{1} $ ')
dR.drawVector(ax, AORG, Dq, arrowstyle='-|>', proj=False, annotationString='Dq')
dR.drawVector(ax, Dq, P2_atA, arrowstyle='-|>', proj=False)
dR.drawVector(ax, AORG, P2_atA, arrowstyle='-|>', proj=False, lineStyle='--', annotationString=' $ ^{A}P_{2} $ ')

ax.set_xlim([0,15]), ax.set_ylim([0,14]), ax.set_zlim([-0.1,5])
ax.set_xlabel('X axis'), ax.set_ylabel('Y axis'), ax.set_zlabel('Z axis')
ax.view_init(azim=-90, elev=90)
plt.show()

그려본 코드는 위와 같습니다.

결과는 이렇게 되네요.. 사실.. 좀 더 편하게 벡터를 3D 공간에서 그리고, 점선, 투영선, annotation 등을 쉽게 할려고 만든 함수를 가지고 이렇게 예제를 통해 잘 만들어 진건지 확인하는 용도였습니다만... 혹시 Craig의 책을 보고 공부하는 분들 중 직접 그래프를 그려볼려는 분들이 있으시다면 도움이 되지 않을까 하고 올려봅니다^^ 그나저나 괜찮은 code highlighter를 찾아야 겠어요.ㅠㅠ.