Craig의 Robotics 3-4예제. RPR Mechanism Arm

요즘 짬 날때마다 Craig라는 분의 Introduction to Robotics 3rd Edition이라는 책을 가지고 놀고 있는데요^^ 그러다가 Craig의 책의 내용에 대한 이해도도 높이고, Python 학습도 하고, 취미삼하는 블로그에 글도 좀 보강하는 여러가지 이유로 책의 예제 중 일부를 Python으로 확인하는 놀이(^^)를 계속하고 있네요. 지난번에 2장의 일부 예제를 한 번 다루었고, 그 후 예제 3-3을 슬라이더를 동원해서 다루었구요.. 오늘은 3-4예제를 이야기할려고 합니다.^^

Theory

좌표계의 기초 표현과 이동 및 회전 행렬

GitHub

PinkWink/drawRobotics

Example

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

Example

Craig의 Robotics 3-3예제. Three-Link Planar Arm

오늘 예제에서 사용되는 기본 함수인 drawRobotics 모듈은 위에 GitHub 주소에서 다운 받으시면 됩니다. 뭐 거기 가시면 예제는 다 있습니다.^^

오늘 다룰 예제는 위 그림과 같이 RPR 메카니즘의 로봇 팔입니다. 회전하고.. 직선 방향으로 움직이고 또 회전하는 형태입니다. 은근 재미있을 것 같단 말이죠^^

원래 예제는 베이스 좌표계인 {0} 좌표계와 {1} 좌표계를 같은 위치에 있다고 해도 되어서 위 그림처럼 Link Parameters를 잡았지만 저는 그려보니 살짝 볼품이 안나서 그냥 link를 하나 추가했습니다. 그래서, {1}좌표계는 베이스에서 L1만큼 x축 방향으로 옮기는 거고, {2}좌표계는 {1}좌표계에서 theta1만큼 z축 중심으로 회전하는 것이 허용되어 있습니다. 그리고 {3}좌표계는 다시 {2}좌표계를 x축 중심으로 90도만큼 돌리고, z축 방향으로 d2의 크기만큼 길이가 가변될 수 있습니다. {4}좌표계는 z축 방향으로 L2만큼 이동한 지점에서 theta3만큼의 각도 변화를 가질 수 있습니다.^^

import sympy as sp

th1 = sp.Symbol('th1')
d2 = sp.Symbol('d2')
th3 = sp.Symbol('th3')
L1 = sp.Symbol('L1')
L2 = sp.Symbol('L2')

def RotZ(a):
	return sp.Matrix( [	[sp.cos(a), -sp.sin(a), 0, 0], 
						[sp.sin(a), sp.cos(a), 0, 0], 
						[0, 0, 1, 0],
						[0, 0, 0, 1] ] )

def RotX(a):
	return sp.Matrix( [	[1, 0, 0, 0], 
						[0, sp.cos(a), -sp.sin(a), 0],
						[0, sp.sin(a), sp.cos(a), 0],
						[0, 0, 0, 1] ] )

def D_q(a1,a2,a3):
	return sp.Matrix([[1,0,0,a1],[0,1,0,a2],[0,0,1,a3],[0,0,0,1]])

Trans_0to1 = D_q(0,0,L1)
Trans_1to2 = RotZ(th1)
Trans_2to3 = RotX(90*sp.pi/180) * D_q(0,0,d2)
Trans_3to4 = D_q(0,0,L2) * RotZ(th3)

Trans_0to2 = sp.simplify(Trans_0to1*Trans_1to2)
Trans_0to3 = sp.simplify(Trans_0to2*Trans_2to3)
Trans_0to4 = sp.simplify(Trans_0to3*Trans_3to4)

print('Trans_0to1 = ')
sp.pprint(Trans_0to1)
print('\n Trans_1to2 = ')
sp.pprint(Trans_1to2)
print('\n Trans_2to3 = ')
sp.pprint(Trans_2to3)
print('\n Trans_3to4 = ')
sp.pprint(Trans_3to4)

print('\n Trans_0to2 = ')
sp.pprint(Trans_0to2)
print('\n Trans_0to3 = ')
sp.pprint(Trans_0to3)
print('\n Trans_0to4 = ')
sp.pprint(Trans_0to4)

Python의 Sympy를 이용하면 문자연산(symbolic)이 가능한데요. 그걸 이용해서 각 변환들을 구했습니다. 위 코드를 실행한 결과는

와 같이 나타납니다.^^. 좀 귀찮긴 하지만... 그래도 정리를 잘 해야 공부한 결과가 되니.. 이를 수식처럼 이쁘게 한 번 정리하면

각 변환이 모두 구해졌습니다.^^. 지난번 3-3 예제[바로가기]처럼 애니메이션 스럽게 구현하는 코드는

import matplotlib.pyplot as plt
import drawRobotics as dR
import numpy as np
from matplotlib.widgets import Slider

axcolor = 'lightgoldenrodyellow'

L1 = 0.5
L2 = 0.2

th1Init = 0
th3Init = 0
d2Init = 1

ORG_0 = np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]])

def calcORGs(a1, a2, a3):
	th1 = dR.conv2Rad(a1)
	d2 = a2
	th3 = dR.conv2Rad(a3)

	Trans_0to1 = dR.D_q(0,0,L1)
	Trans_1to2 = np.r_[np.c_[dR.RotZ(th1), np.zeros(3)], [[0,0,0,1]]]
	Trans_2to3 = np.dot( np.r_[np.c_[dR.RotX(dR.conv2Rad(90)), np.zeros(3)], [[0,0,0,1]]], dR.D_q(0,0,d2) )
	Trans_3to4 = np.dot( dR.D_q(0,0,L2), np.r_[np.c_[dR.RotZ(th3), np.zeros(3)], [[0,0,0,1]]] )

	Trans_0to2 = np.dot(Trans_0to1, Trans_1to2)
	Trans_0to3 = np.dot(Trans_0to2, Trans_2to3)
	Trans_0to4 = np.dot(Trans_0to3, Trans_3to4)

	ORG_1 = np.dot(Trans_0to1, ORG_0)
	ORG_2 = np.dot(Trans_0to2, ORG_0)
	ORG_3 = np.dot(Trans_0to3, ORG_0)
	ORG_4 = np.dot(Trans_0to4, ORG_0)

	return ORG_1, ORG_2, ORG_3, ORG_4

def drawObject(ORG_1, ORG_2, ORG_3, ORG_4):
	dR.drawPointWithAxis(ax, ORG_0, lineStyle='--', vectorLength=1, lineWidth=2)
	dR.drawPointWithAxis(ax, ORG_1, vectorLength=0.5)
	dR.drawPointWithAxis(ax, ORG_2, vectorLength=0.5)
	dR.drawPointWithAxis(ax, ORG_3, vectorLength=0.5)
	dR.drawPointWithAxis(ax, ORG_4, vectorLength=0.5, lineWidth=2)

	dR.drawVector(ax, ORG_0, ORG_1, arrowstyle='-', lineColor='c', proj=False, lineWidth=10)
	dR.drawVector(ax, ORG_1, ORG_2, arrowstyle='-', lineColor='k', proj=False, lineWidth=8)
	dR.drawVector(ax, ORG_2, ORG_3, arrowstyle='-', lineColor='k', proj=False, lineWidth=4)
	dR.drawVector(ax, ORG_3, ORG_4, arrowstyle='-', lineColor='k', proj=False, lineWidth=2)

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

def update(val):
	th1 = s1Angle.val
	d2 = s2Length.val
	th3 = s3Angle.val

	ORG_1, ORG_2, ORG_3, ORG_4 = calcORGs(th1, d2, th3)

	ax.cla()

	drawObject(ORG_1, ORG_2, ORG_3, ORG_4)
	
ORG_1, ORG_2, ORG_3, ORG_4 = calcORGs(th1Init, d2Init, th3Init)

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

th1Angle = plt.axes([0.125, 0.16, 0.77, 0.03], axisbg=axcolor)
d2Length = plt.axes([0.125, 0.10, 0.77, 0.03], axisbg=axcolor)
th3Angle = plt.axes([0.125, 0.04, 0.77, 0.03], axisbg=axcolor)

s1Angle = Slider(th1Angle, r'$ \theta_1 $', -180.0, 180.0, valinit=th1Init)
s2Length = Slider(d2Length, r'$ d_2 $', 0.5, 2.0, valinit=d2Init)
s3Angle = Slider(th3Angle, r'$ \theta_3 $', -180.0, 180.0, valinit=th3Init)

drawObject(ORG_1, ORG_2, ORG_3, ORG_4)
	
s1Angle.on_changed(update)
s2Length.on_changed(update)
s3Angle.on_changed(update)

ax.view_init(azim=-150, elev=30)
plt.show()

지난번 3-3 예제 보다는 좀 간결해 졌다고 이야기해주세요^^

위와 같이 실행됩니다. theta1/theta3와 d2를 조절할 수 있습니다.

이렇게 학습한 내용을 직접 눈으로 보면 공부하는데 도움이 되죠^^ 저말이죠.. 저한테. ㅎㅎ^^. 취미삼아하는 일이었는데.. 살짝 피곤하네요^^.