본문 바로가기
Theory/ControlTheory

Craig의 Robotics 3장 예제. PUMA 560 Python으로 확인해보기

PinkWink 2016. 2. 3. 08:30

Craig의 Introduction to Robotics 3rd Edition를 대상으로 예제의 시뮬레이션 중 만만한(^^) 아이들을 Python으로 구현해보는 놀이를 나 혼자만의 세계에 빠져서 취미처럼 수행했는데요... 최근 2장을 마치고.. 이제 3장의 마지막 PUMA 560을 다룰려고 합니다.~ 아마 3장의 메인 예제라고 해도 될겁니다. Craig의 책 3장에서는 이 예제를 이해했다면 2장 3장의 내용은 다 이해했다고 봐도 되겠죠^^

Theory
좌표계의 기초 표현과 이동 및 회전 행렬
GitHub
PinkWink/drawRobotics
Example
Python으로 학습해본 Craig의 Robotics 2장 예제
Example
Craig의 Robotics 3-3예제. Three-Link Planar Arm
Example
Craig의 Robotics 3-4예제. RPR Mechanism Arm

출처 : Introduction to Robotics 3rd Edition by Craig

PUMA560은 Programmable Universal Machine for Assembly (or Programmable Universal Manipulation Arm)이라는 이름의 약자입니다. 1980년대 초에 개발되었으니 상상해보면 지금과 같은 PC 환경이 아니라는 거죠... 그런걸 생각하면 참 잘 만들어진 로봇인듯합니다.

{4}번 좌표계까지의 기구 구성 출처 : Introduction to Robotics 3rd Edition by Craig

PUMA560은 위 그림처럼 크게 회전하는 축으로 3개가 구성되어 있으며

나머지 6번 좌표계까지의 기구구성 출처 : Introduction to Robotics 3rd Edition by Craig

툴 끝단의 3축 구현을 위한 3개의 축이 구성되어 있습니다.

PUMA560의 Link Parameter 출처 : Introduction to Robotics 3rd Edition by Craig

그래서 위 그림과 같이 Link Parameter를 구성할 수 있습니다. 이제 저런 모델만 되어도 이를 수식으로 표현하는 것은 살짝 힘겹습니다. 뭐 물론 여차하면 손으로 유도할 수 있는 소양(^^)은 갖추어야겠지만 말이죠^^

import sympy as sp
sp.init_printing()

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

theta1 = sp.Symbol('theta1')
theta2 = sp.Symbol('theta2')
theta3 = sp.Symbol('theta3')
theta4 = sp.Symbol('theta4')
theta5 = sp.Symbol('theta5')
theta6 = sp.Symbol('theta6')

a2, a3 = sp.symbols('a2 a3')
d3, d4 = sp.symbols('d3 d4')

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

위 코드는 IPython에서 SymPy를 사용해서 문자연산으로 위의 기구학 모델을 수식으로 표현할려고 한 겁니다. 이때 sp.init_printing()은 수식을 이쁘게(^^) 보여주기 위한 준비 명령입니다.^^

Trans_0to1 = RotZ(theta1)
Trans_1to2 = RotX(conv2Rad(-90))*RotZ(theta2)
Trans_2to3 = D_q(a2,0,0)*D_q(0,0,d3)*RotZ(theta3)
Trans_3to4 = RotX(conv2Rad(-90))*D_q(a3,0,0)*D_q(0,0,d4)*RotZ(theta4)
Trans_4to5 = RotX(conv2Rad(90))*RotZ(theta5)
Trans_5to6 = RotX(conv2Rad(-90))*RotZ(theta6)

Trans_0to2 = sp.simplify(Trans_0to1*Trans_1to2)
Trans_0to3 = sp.simplify(Trans_0to2*Trans_2to3)
Trans_0to4 = sp.simplify(Trans_0to3*Trans_3to4)
Trans_0to5 = sp.simplify(Trans_0to4*Trans_4to5)
Trans_0to6 = sp.simplify(Trans_0to5*Trans_5to6)

위의 표현이 전부입니다. 쉽죠?^^ Link Parameter에 기술된 내용대로 T_0_1, ... T_5_6을 정의하고, 원점부터 각 링크까지의 정보를 계산한겁니다. 

그러면 IPython에서는 저렇게 결과가 나오지요~~~

각 링크에서 다음 링크로 가는 변환은 손으로 쉽게 되지만.. 나머지는 위에 이야기한 IPython의 도움을 받아야죠^^

흠... 뭐 코드를 돌리면 나오는거니.. 워드같은걸로 이쁘게 정리하지는 않겠습니다.(라고 쓰고.. 너~무 귀찮습니다.^^ 요즘은 이런 수식 전체를 학위논문에서 조차도 보여주지 않더군요. 사실 결과식으로 뭔가 다른 의미있는 행동을 하는 것이 아니라면 큰 필요가 없죠.. 눈만 아프고^^) 뭐 아무튼... 이제 시뮬레이션 놀이 해야죠^^

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

axcolor = 'lightgoldenrodyellow'

th1Init, th2Init, th3Init, th4Init, th5Init, th6Init = 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

a2 = 1.5
a3 = 0.5
d3 = 0.5
d4 = 1.5

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

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

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

def D_q(dq1,dq2,dq3):
    return np.array([[1,0,0,dq1],[0,1,0,dq2],[0,0,1,dq3],[0,0,0,1]])


def calcORGs(q1, q2, q3, q4, q5, q6):
    th1 = dR.conv2Rad(q1)
    th2 = dR.conv2Rad(q2)
    th3 = dR.conv2Rad(q3)
    th4 = dR.conv2Rad(q4)
    th5 = dR.conv2Rad(q5)
    th6 = dR.conv2Rad(q6)

    Trans_0to1 = RotZ(th1)
    Trans_1to2 = np.dot(RotX(dR.conv2Rad(-90)), RotZ(th2))
    Trans_2to3 = np.dot(np.dot(D_q(a2,0,0), D_q(0,0,d3)), RotZ(th3))
    Trans_3to4 = np.dot(np.dot(np.dot(RotX(dR.conv2Rad(-90)), D_q(a3,0,0)), D_q(0,0,d4)), RotZ(th4))
    Trans_4to5 = np.dot(RotX(dR.conv2Rad(90)), RotZ(th5))
    Trans_5to6 = np.dot(RotX(dR.conv2Rad(-90)), RotZ(th6))

    Trans_0to2 = np.dot(Trans_0to1, Trans_1to2)
    Trans_0to3 = np.dot(Trans_0to2, Trans_2to3)
    Trans_0to4 = np.dot(Trans_0to3, Trans_3to4)
    Trans_0to5 = np.dot(Trans_0to4, Trans_4to5)
    Trans_0to6 = np.dot(Trans_0to5, Trans_5to6)

    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)
    ORG_5 = np.dot(Trans_0to5, ORG_0)
    ORG_6 = np.dot(Trans_0to6, ORG_0)

    return ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6

def drawObject(ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6):
    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)
    dR.drawPointWithAxis(ax, ORG_5, vectorLength=0.5)
    dR.drawPointWithAxis(ax, ORG_6, vectorLength=0.5)

    dR.drawVector(ax, ORG_Base, ORG_0, arrowstyle='-', lineColor='c', proj=False, lineWidth=5)
    dR.drawVector(ax, ORG_0, ORG_1, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_1, ORG_2, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_2, ORG_3, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_3, ORG_4, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_4, ORG_5, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_5, ORG_6, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)

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

def update(val):
    th1 = s1Angle.val
    th2 = s2Angle.val
    th3 = s3Angle.val
    th4 = s4Angle.val
    th5 = s5Angle.val
    th6 = s6Angle.val

    ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6 = calcORGs(th1, th2, th3, th4, th5, th6)

    ax.cla()

    drawObject(ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6)
    
ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6 = calcORGs(th1Init, th2Init, th3Init, th4Init, th5Init, th6Init)

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

th1Angle = plt.axes([0.125, 0.16, 0.34, 0.03], axisbg=axcolor)
th2Angle = plt.axes([0.125, 0.10, 0.34, 0.03], axisbg=axcolor)
th3Angle = plt.axes([0.125, 0.04, 0.34, 0.03], axisbg=axcolor)
th4Angle = plt.axes([0.55, 0.16, 0.34, 0.03], axisbg=axcolor)
th5Angle = plt.axes([0.55, 0.10, 0.34, 0.03], axisbg=axcolor)
th6Angle = plt.axes([0.55, 0.04, 0.34, 0.03], axisbg=axcolor)

s1Angle = Slider(th1Angle, r'$ \theta_1 $', -180.0, 180.0, valinit=th1Init)
s2Angle = Slider(th2Angle, r'$ \theta_2 $', -180.0, 180.0, valinit=th2Init)
s3Angle = Slider(th3Angle, r'$ \theta_3 $', -180.0, 180.0, valinit=th3Init)
s4Angle = Slider(th4Angle, r'$ \theta_4 $', -180.0, 180.0, valinit=th4Init)
s5Angle = Slider(th5Angle, r'$ \theta_5 $', -180.0, 180.0, valinit=th5Init)
s6Angle = Slider(th6Angle, r'$ \theta_6 $', -180.0, 180.0, valinit=th6Init)

drawObject(ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6)

s1Angle.on_changed(update)
s2Angle.on_changed(update)
s3Angle.on_changed(update)
s4Angle.on_changed(update)
s5Angle.on_changed(update)
s6Angle.on_changed(update)

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

바로 이전에 보여드렸던 2장 3장의 예제처럼 같은 스타일로 작성했습니다. 그 결과를 보면

이렇게 보이네요.. 뭔가 약간 어색하죠?

마찬가지로 약간 어색합니다. 이유는 바로 위의 Link Parameter의 3번 4번 좌표계 때문인데요. 회전과 이동이 한 번 들어가는 건 괜찮은데.. 이동이 두번 들어간다는 것은 쉽게 말해서 사각형의 대각선으로 이동한 것을 의미합니다. Link 표현은 줄어들지만 이를 시뮬레이션해보면 약간 어색한 거죠. 그래서 Link Parameter를 약간 수정합니다.

이렇게 추가하는 거죠... Link 기술에서 한 단계에 이동이 두 번 들어가지 않도록... (아.. 빈칸은 '0'입니다.) 그렇게 다시 코드를 만들면

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

axcolor = 'lightgoldenrodyellow'

th1Init, th2Init, th3Init, th4Init, th5Init, th6Init = 0.0, 0.0, 0.0, 0.0, 0.0, 0.0

a2 = 1.5
a3 = 0.5
d3 = 0.5
d4 = 1.5

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

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

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

def D_q(dq1,dq2,dq3):
    return np.array([[1,0,0,dq1],[0,1,0,dq2],[0,0,1,dq3],[0,0,0,1]])


def calcORGs(q1, q2, q3, q4, q5, q6):
    th1 = dR.conv2Rad(q1)
    th2 = dR.conv2Rad(q2)
    th3 = dR.conv2Rad(q3)
    th4 = dR.conv2Rad(q4)
    th5 = dR.conv2Rad(q5)
    th6 = dR.conv2Rad(q6)

    Trans_0to1 = RotZ(th1)
    Trans_1to2 = np.dot(RotX(dR.conv2Rad(-90)), RotZ(th2))
    Trans_2to3 = D_q(a2,0,0)
    Trans_3to4 = np.dot(D_q(0,0,d3), RotZ(th3))
    Trans_4to5 = np.dot(RotX(dR.conv2Rad(-90)), D_q(a3,0,0))
    Trans_5to6 = np.dot(D_q(0,0,d4), RotZ(th4))
    Trans_6to7 = np.dot(RotX(dR.conv2Rad(90)), RotZ(th5))
    Trans_7to8 = np.dot(RotX(dR.conv2Rad(-90)), RotZ(th6))

    Trans_0to2 = np.dot(Trans_0to1, Trans_1to2)
    Trans_0to3 = np.dot(Trans_0to2, Trans_2to3)
    Trans_0to4 = np.dot(Trans_0to3, Trans_3to4)
    Trans_0to5 = np.dot(Trans_0to4, Trans_4to5)
    Trans_0to6 = np.dot(Trans_0to5, Trans_5to6)
    Trans_0to7 = np.dot(Trans_0to6, Trans_6to7)
    Trans_0to8 = np.dot(Trans_0to7, Trans_7to8)

    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)
    ORG_5 = np.dot(Trans_0to5, ORG_0)
    ORG_6 = np.dot(Trans_0to6, ORG_0)
    ORG_7 = np.dot(Trans_0to7, ORG_0)
    ORG_8 = np.dot(Trans_0to8, ORG_0)

    return ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6, ORG_7, ORG_8

def drawObject(ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6, ORG_7, ORG_8):
    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)
    dR.drawPointWithAxis(ax, ORG_5, vectorLength=0.5)
    dR.drawPointWithAxis(ax, ORG_6, vectorLength=0.5)
    dR.drawPointWithAxis(ax, ORG_7, vectorLength=0.5)
    dR.drawPointWithAxis(ax, ORG_8, vectorLength=0.5)

    dR.drawVector(ax, ORG_Base, ORG_0, arrowstyle='-', lineColor='c', proj=False, lineWidth=5)
    dR.drawVector(ax, ORG_0, ORG_1, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_1, ORG_2, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_2, ORG_3, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_3, ORG_4, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_4, ORG_5, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_5, ORG_6, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_6, ORG_7, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawVector(ax, ORG_7, ORG_8, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)

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

def update(val):
    th1 = s1Angle.val
    th2 = s2Angle.val
    th3 = s3Angle.val
    th4 = s4Angle.val
    th5 = s5Angle.val
    th6 = s6Angle.val

    ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6, ORG_7, ORG_8 = calcORGs(th1, th2, th3, th4, th5, th6)

    ax.cla()

    drawObject(ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6, ORG_7, ORG_8)
    
ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6, ORG_7, ORG_8 = calcORGs(th1Init, th2Init, th3Init, th4Init, th5Init, th6Init)

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

th1Angle = plt.axes([0.125, 0.16, 0.34, 0.03], axisbg=axcolor)
th2Angle = plt.axes([0.125, 0.10, 0.34, 0.03], axisbg=axcolor)
th3Angle = plt.axes([0.125, 0.04, 0.34, 0.03], axisbg=axcolor)
th4Angle = plt.axes([0.55, 0.16, 0.34, 0.03], axisbg=axcolor)
th5Angle = plt.axes([0.55, 0.10, 0.34, 0.03], axisbg=axcolor)
th6Angle = plt.axes([0.55, 0.04, 0.34, 0.03], axisbg=axcolor)

s1Angle = Slider(th1Angle, r'$ \theta_1 $', -180.0, 180.0, valinit=th1Init)
s2Angle = Slider(th2Angle, r'$ \theta_2 $', -180.0, 180.0, valinit=th2Init)
s3Angle = Slider(th3Angle, r'$ \theta_3 $', -180.0, 180.0, valinit=th3Init)
s4Angle = Slider(th4Angle, r'$ \theta_4 $', -180.0, 180.0, valinit=th4Init)
s5Angle = Slider(th5Angle, r'$ \theta_5 $', -180.0, 180.0, valinit=th5Init)
s6Angle = Slider(th6Angle, r'$ \theta_6 $', -180.0, 180.0, valinit=th6Init)

drawObject(ORG_1, ORG_2, ORG_3, ORG_4, ORG_5, ORG_6, ORG_7, ORG_8)

s1Angle.on_changed(update)
s2Angle.on_changed(update)
s3Angle.on_changed(update)
s4Angle.on_changed(update)
s5Angle.on_changed(update)
s6Angle.on_changed(update)

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

에고.. 뭐 길긴 하지만. 그리 어려운 구현은 아닙니다.^^

조금더 눈에 보기 편하네요.. 언제 시간나면 기구적 3D 도면 정보를 그대로 화면에 뿌려보고 싶네요~~~

이렇게 Craig의 교재 3장을 마쳤네요~~ 이제 4장으로 가야죠. ㅎㅎㅎ...

반응형

'Theory > ControlTheory' 카테고리의 다른 글

역 Z-변환된 차분 방정식을 C 코드로 계산하기  (4) 2016.07.22
Craig의 Robotics 5장. Jacobian 자코비안  (25) 2016.03.18
Craig의 Robotics 4장 예제. PUMA 560의 역기구학 풀이 (Inverse Kinematics)  (14) 2016.02.17
Craig의 Robotics 3-4예제. RPR Mechanism Arm  (20) 2016.01.22
Craig의 Robotics 3-3예제. Three-Link Planar Arm  (10) 2016.01.20
Python으로 학습해본 Craig의 Robotics 2장 예제  (4) 2015.12.23
좌표계의 기초 표현과 이동 및 회전 행렬  (26) 2015.11.26

'Theory/ControlTheory' Related Articles

티스토리툴바