Python에서 좌표계와 벡터 그리기

요즘 갑자기 아기 미바뤼가 아프답니다. 아직 폐렴까지 진행하지는 않았지만 걱정된다고 하네요... 덩달아 아빠는 잠을 잘 수가 없습니다. 혹시 기침하다가 아가가 중간에 깰까바...ㅠㅠ. 그래서 지금까지 취미삼아 데리고 놀던 것을 살짝 블로그를 할려고 아가 미바뤼가 잘 보이는 곳에 어두 컴컴한 곳에 앉아서 이렇게 블로그질~(^^) 중입니다. 흠...

최근 저는 로보틱스에서 정~말 기초가 되는 좌표계에 대한 이야기를 했었는데요.[바로가기] 그러면서 이런 개념을 3D 그래프로 직접 그려보고 싶다는 생각을 하게 된거죠.. 문제는 제가 그런 그림그리기는 아~~주 약하다는.ㅠㅠ. 그러다가 인터넷을 찾다가 공부하고 (아무도 신경쓰지 않지만) 혼자 좋아라 하면서 작업한 것을 살짝 올리려는 것입니다.^^ 일단 로보틱스적인 것을 그림으로 표현할 때 가장 기초이면서 또 먼저 필요한 것이.. 흠.. 좀 생뚱맞을 수도 있지만... ㅎ.. 화살표입니다.~^^ MATLAB에서는 제가 편하게 quiver3[바로가기]라는 함수를 사용했었는데요... 사실 이 함수는 그럴 용도도 아니지만.. 3D로 표현된 화살표가 아닙니다.^^. 당연히 Python에도 quiver가 있는데요. (default 설정이 좀 다르긴 하지만^^) 이번에는 요것 말고.. 화면을 이리저리 돌려도 화살표가 잘나오는 것을 찾고 싶었던거죠.. ㅎ.. 그러다가 당연하지만.. 인터넷의 바다에서 쉽게 찾았답니다. 그런데.. 이 함수의 원 출처를 모르겠더군요. 그래서 출처없이 그냥 공개합니다...^^.

class Arrow3D(FancyArrowPatch):
    def __init__(self, xs, ys, zs, *args, **kwargs):
        FancyArrowPatch.__init__(self, (0,0), (0,0), *args, **kwargs)
        self._verts3d = xs, ys, zs

    def draw(self, renderer):
        xs3d, ys3d, zs3d = self._verts3d
        xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M)
        self.set_positions((xs[0],ys[0]),(xs[1],ys[1]))
        FancyArrowPatch.draw(self, renderer)

바뤼 위 클래스입니다. 그냥.. 화살표를 3D로 잘 랜더링해서 항상.. 어느 각도에서도 화살표이게~~ 해주는 함수랍니다.^^ 이제 이걸 이용해서.. 제가 몇 개의 함수를 만들었습니다. 왜냐구요?? 흠.. 그냥.. 재미있어서요^^. 당연한 이야기지만.. Python 완전 초보가 만든 것이기 때문에.. 뭐 퀄리티~~ 요딴걸 기대하시면 안됩니다. ㅋㅋ

def drawVector(fig, pointA, pointB, **kwargs):
	ms = kwargs.get('mutation_scale', 20)
	ars = kwargs.get('arrowstyle', '-|>')
	lc = kwargs.get('lineColor', 'k')
	pc = kwargs.get('projColor', 'k')
	pointEnable = kwargs.get('pointEnable', True)
	projOn = kwargs.get('proj', True)

	if pointA.size == 3:
		xs = [pointA[0], pointB[0]]
		ys = [pointA[1], pointB[1]]
		zs = [pointA[2], pointB[2]]
	else:
		xs = [pointA[0,3], pointB[0,3]]
		ys = [pointA[1,3], pointB[1,3]]
		zs = [pointA[2,3], pointB[2,3]]

	out = Arrow3D(xs, ys, zs, mutation_scale=ms, arrowstyle=ars, color=lc)
	fig.add_artist(out)

	if pointEnable: fig.scatter(xs[1], ys[1], zs[1], color='k', s=50)
	if projOn==True:
		fig.plot(xs, ys, [0, 0], color=pc, linestyle='--')
		fig.plot([xs[0], xs[0]], [ys[0], ys[0]], [0, zs[0]], color=pc, linestyle='--')
		fig.plot([xs[1], xs[1]], [ys[1], ys[1]], [0, zs[1]], color=pc, linestyle='--')

먼저 만든 것은 drawVector 함수입니다. 흠.. 말 그대로 벡터를 그리는 것 입니다. 그런데 왜 이리 기냐구요?^^. 그건 제가 [바로가기]에서도 이야기했지만.. 3차원을 가지는 좌표(크가가 3)로 벡터를 표현하는 경우도 있고, homogeneous transform으로 표현(크기가 4*4)하는 경우도 있기 때문에.. 일단 주어진 점의 크기를 가늠해야 했습니다. 그리고, 제일 위에 인터넷에서 주웠다는 그 함수 Arrow3D를 사용해서 벡터를 그린답니다. 그리고.. 이 함수는 **kwargs [바로가기]로 몇 몇 설정을 할 수 있는데요. 먼저 기본적인 화살표 스타일과, 크기, 색상 등을 설정하고 있습니다. 그리고, 3D 공간에서 표현되기 때문에 xy 평면에 정사영을 만들어서 구분을 쉽게 하도록 하고 있는데요. 기본값은 이걸 사용하는 것입니다. 필요없으면 proj='False'로 하면 사라집니다. 그리고, 화살표의 도착지점에 원으로 점을 찍을지를 결정하는 옵션은 pointEnable입니다.

def drawPointWithAxis(fig, *args, **kwargs):
	ms = kwargs.get('mutation_scale', 20)
	ars = kwargs.get('arrowstyle', '->')
	pointEnable = kwargs.get('pointEnable', True)
	axisEnable = kwargs.get('axisEnable', True)

	if len(args) == 4:
		ORG = args[0]
		hat_X = args[1]
		hat_Y = args[2]
		hat_Z = args[3]
		xs_n = [ORG[0], ORG[0] + hat_X[0]]
		ys_n = [ORG[1], ORG[1] + hat_X[1]]
		zs_n = [ORG[2], ORG[2] + hat_X[2]]
		xs_o = [ORG[0], ORG[0] + hat_Y[0]]
		ys_o = [ORG[1], ORG[1] + hat_Y[1]]
		zs_o = [ORG[2], ORG[2] + hat_Y[2]]
		xs_a = [ORG[0], ORG[0] + hat_Z[0]]
		ys_a = [ORG[1], ORG[1] + hat_Z[1]]
		zs_a = [ORG[2], ORG[2] + hat_Z[2]]
	else:
		tmp = args[0]
		ORG = tmp[:3,3:]
		hat_X = tmp[:3,0:1]
		hat_Y = tmp[:3,1:2]
		hat_Z = tmp[:3,2:3]
		xs_n = [ORG[0, 0], ORG[0, 0] + hat_X[0, 0]]
		ys_n = [ORG[1, 0], ORG[1, 0] + hat_X[1, 0]]
		zs_n = [ORG[2, 0], ORG[2, 0] + hat_X[2, 0]]
		xs_o = [ORG[0, 0], ORG[0, 0] + hat_Y[0, 0]]
		ys_o = [ORG[1, 0], ORG[1, 0] + hat_Y[1, 0]]
		zs_o = [ORG[2, 0], ORG[2, 0] + hat_Y[2, 0]]
		xs_a = [ORG[0, 0], ORG[0, 0] + hat_Z[0, 0]]
		ys_a = [ORG[1, 0], ORG[1, 0] + hat_Z[1, 0]]
		zs_a = [ORG[2, 0], ORG[2, 0] + hat_Z[2, 0]]

	if pointEnable: fig.scatter(xs_n[0], ys_n[0], zs_n[0], color='k', s=50)

	if axisEnable:
		n = Arrow3D(xs_n, ys_n, zs_n, mutation_scale=ms, arrowstyle=ars, color='r')
		o = Arrow3D(xs_o, ys_o, zs_o, mutation_scale=ms, arrowstyle=ars, color='g')
		a = Arrow3D(xs_a, ys_a, zs_a, mutation_scale=ms, arrowstyle=ars, color='b')
		fig.add_artist(n)
		fig.add_artist(o)
		fig.add_artist(a)

이제 drawPointWithAxis 함수입니다. 이 아이도 뭐 단순합니다. 공간상에 점을 찍을건데.. 그 방위도 표현할려고 한 것입니다. 마찬가지로, 점을 표시할 것인지, 축을 표시할 것인지를 정할 수 있습니다. 역시 drawVector와 마찬가지로, 주어진 입력이 좌표와 방향벡터들인지, 혹은 4*4의 homogeneous transform의 형태인지를 확인하도록 하고 있습니다.

<drawRobotics.py>

from matplotlib import pyplot as plt
from matplotlib.patches import FancyArrowPatch
from mpl_toolkits.mplot3d import proj3d
import numpy as np

conv2Rad = lambda x : x*np.pi/180

class Arrow3D(FancyArrowPatch):
    def __init__(self, xs, ys, zs, *args, **kwargs):
        FancyArrowPatch.__init__(self, (0,0), (0,0), *args, **kwargs)
        self._verts3d = xs, ys, zs

    def draw(self, renderer):
        xs3d, ys3d, zs3d = self._verts3d
        xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M)
        self.set_positions((xs[0],ys[0]),(xs[1],ys[1]))
        FancyArrowPatch.draw(self, renderer)

def drawVector(fig, pointA, pointB, **kwargs):
	ms = kwargs.get('mutation_scale', 20)
	ars = kwargs.get('arrowstyle', '-|>')
	lc = kwargs.get('lineColor', 'k')
	pc = kwargs.get('projColor', 'k')
	pointEnable = kwargs.get('pointEnable', True)
	projOn = kwargs.get('proj', True)

	if pointA.size == 3:
		xs = [pointA[0], pointB[0]]
		ys = [pointA[1], pointB[1]]
		zs = [pointA[2], pointB[2]]
	else:
		xs = [pointA[0,3], pointB[0,3]]
		ys = [pointA[1,3], pointB[1,3]]
		zs = [pointA[2,3], pointB[2,3]]

	out = Arrow3D(xs, ys, zs, mutation_scale=ms, arrowstyle=ars, color=lc)
	fig.add_artist(out)

	if pointEnable: fig.scatter(xs[1], ys[1], zs[1], color='k', s=50)
	if projOn==True:
		fig.plot(xs, ys, [0, 0], color=pc, linestyle='--')
		fig.plot([xs[0], xs[0]], [ys[0], ys[0]], [0, zs[0]], color=pc, linestyle='--')
		fig.plot([xs[1], xs[1]], [ys[1], ys[1]], [0, zs[1]], color=pc, linestyle='--')
		
def drawPointWithAxis(fig, *args, **kwargs):
	ms = kwargs.get('mutation_scale', 20)
	ars = kwargs.get('arrowstyle', '->')
	pointEnable = kwargs.get('pointEnable', True)
	axisEnable = kwargs.get('axisEnable', True)

	if len(args) == 4:
		ORG = args[0]
		hat_X = args[1]
		hat_Y = args[2]
		hat_Z = args[3]
		xs_n = [ORG[0], ORG[0] + hat_X[0]]
		ys_n = [ORG[1], ORG[1] + hat_X[1]]
		zs_n = [ORG[2], ORG[2] + hat_X[2]]
		xs_o = [ORG[0], ORG[0] + hat_Y[0]]
		ys_o = [ORG[1], ORG[1] + hat_Y[1]]
		zs_o = [ORG[2], ORG[2] + hat_Y[2]]
		xs_a = [ORG[0], ORG[0] + hat_Z[0]]
		ys_a = [ORG[1], ORG[1] + hat_Z[1]]
		zs_a = [ORG[2], ORG[2] + hat_Z[2]]
	else:
		tmp = args[0]
		ORG = tmp[:3,3:]
		hat_X = tmp[:3,0:1]
		hat_Y = tmp[:3,1:2]
		hat_Z = tmp[:3,2:3]
		xs_n = [ORG[0, 0], ORG[0, 0] + hat_X[0, 0]]
		ys_n = [ORG[1, 0], ORG[1, 0] + hat_X[1, 0]]
		zs_n = [ORG[2, 0], ORG[2, 0] + hat_X[2, 0]]
		xs_o = [ORG[0, 0], ORG[0, 0] + hat_Y[0, 0]]
		ys_o = [ORG[1, 0], ORG[1, 0] + hat_Y[1, 0]]
		zs_o = [ORG[2, 0], ORG[2, 0] + hat_Y[2, 0]]
		xs_a = [ORG[0, 0], ORG[0, 0] + hat_Z[0, 0]]
		ys_a = [ORG[1, 0], ORG[1, 0] + hat_Z[1, 0]]
		zs_a = [ORG[2, 0], ORG[2, 0] + hat_Z[2, 0]]

	if pointEnable: fig.scatter(xs_n[0], ys_n[0], zs_n[0], color='k', s=50)

	if axisEnable:
		n = Arrow3D(xs_n, ys_n, zs_n, mutation_scale=ms, arrowstyle=ars, color='r')
		o = Arrow3D(xs_o, ys_o, zs_o, mutation_scale=ms, arrowstyle=ars, color='g')
		a = Arrow3D(xs_a, ys_a, zs_a, mutation_scale=ms, arrowstyle=ars, color='b')
		fig.add_artist(n)
		fig.add_artist(o)
		fig.add_artist(a)

def RotX(phi):
    return np.array([[1, 0, 			0],
                     [0, np.cos(phi), 	-np.sin(phi)],
                     [0, np.sin(phi), 	np.cos(phi)]])

def RotY(theta):
    return np.array([[np.cos(theta), 	0, np.sin(theta)],
                     [0, 				1, 0],
                     [-np.sin(theta), 	0, np.cos(theta)]])

def RotZ(psi):
    return np.array([[np.cos(psi), 	-np.sin(psi), 	0],
                     [np.sin(psi), 	np.cos(psi), 	0],
                     [0, 			0, 				1]])

위에 drawRobotics.py의 전체 코드가 있습니다. 아직 회전행렬들은 그냥 그대로 이지만.. 뭐 그건 차츰차츰 완료해 가도록 하죠^^. 아무튼 이렇게 module 하나를 만들었네요^^ 이제 이 아이를 사용하는 예제를 보도록 하겠습니다.

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

P_atA = np.array([2.2,2.2,1.5])
BORG = np.array( [ 	[-1, 0, 0, 1],
					[0, -1, 0, 1.5],
					[0, 0, -1, 2],
					[0, 0, 0, 1]])

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])

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.drawVector(ax, AORG, P_atA, arrowstyle='-|>')
dR.drawPointWithAxis(ax, BORG)

ax.set_xlim([-0.1,2.5]), ax.set_ylim([-0.1,2.5]), ax.set_zlim([-0.1,2.5])
ax.set_xlabel('X axis'), ax.set_ylabel('Y axis'), ax.set_zlabel('Z axis')
plt.show()

위가 사용 예제입니다. P_atA는 그냥 크기가 3인 공간의 점을 표현하지만, BORG는 또 4*4의 크기를 가지고 방향벡터와 자신의 원점 정보를 모두 가지고 있네요. 아무튼 import drawRobotics as dR로 방금 허접하게 만든 모듈을 import하고... 사용하시면 됩니다. 뭐 원체 간단하니 그 결과만 보시죠^^

요렇게 나타납니다. 뭐.. 제가 원하던 성능이랍니다. ㅎㅎㅎ. 아무튼. 요건 항상 github[바로가기]에서 업데이트를 시간나면 하든지 말든지 할 예정이랍니다.^^

'Software > Python' 카테고리의 다른 글

Python Matplotlib의 Slider 위젯을 적용한 간편 예제 (4)	2016.01.13
Python Matplotlib를 이용해서 간단하게 GUI의 슬라이더, 라디오버튼, 버튼을 구현하는 예제 (21)	2016.01.08
Python에서 벡터 그리기 drawRobotics Ver0.01 (8)	2015.12.17
Python을 이용한 위치에서 속도를 구하는 여러가지 방법에 대한 예제 (7)	2015.08.26
Python Pandas를 이용해 분석한 데이터를 그래프로 표현하기... (6)	2015.08.06
Python - IPython에서 구현하는 저역통과필터 Low Pass Filter (14)	2015.05.15
Python pandas를 이용한 저장된 파일에서 읽은 데이터를 쉽게 사용하기 (26)	2015.05.06

PinkWink

Python에서 좌표계와 벡터 그리기

'Software > Python' 카테고리의 다른 글

티스토리툴바

Python에서 좌표계와 벡터 그리기

'Software > Python' 카테고리의 다른 글

'Software/Python' Related Articles

티스토리툴바