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

최근 얼마전부터 Craig의 Introduction to Robotics 3rd Edition의 2장 예제 중 일부를 Python으로 구현해서 실습했는데요. 오늘은 Craig의 책 3장의 3-3예제를 가지고 실습을 할까합니다. 일단, 로보틱스의 기초적 지식은 이미 [바로가기]에서 언급을 했구요... 그후 2장의 예제를 다루기 전에 벡터나 좌표계를 공간상에 표현할 방법을 찾다가 살짝 직접 구현을 했었죠[바로가기]. 그리고 그후 해당 Python Module은 살짝 변경이 생겨서 지금은 GitHub에서 확인을 하셔야합니다.[바로가기] 그리고 Craig책 2장 예제 중 일부를 이야기 했었죠[바로가기]^^ 그렇게 해서 오늘 글에서 사용할 링크까지 정리하면

Theory

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

GitHub

PinkWink/drawRobotics

Example

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

Example

Python Matplotlib를 이용해서 간단하게 GUI의 슬라이더, 라디오버튼, 버튼을 구현하는 예제

Example

Python Matplotlib의 Slider 위젯을 적용한 간편 예제

Example

Numpy에서 선언된 행렬에 c_나 r_를 이용해서 행이나 렬 쉽게 추가하기

와 같습니다.^^ 아무튼... 오늘은 3장에 나오는 예제 하나를 .. 아주아주 간단한 예제 하나를 다뤄보도록 하죠^^

출처 : Introduction to Robotics 3rd Edition by Craig. Example 3-3

위 예제에도 나와 있지만, Three Link Planar는 모두 Revolute인 축만으로 이뤄져 있어서 흔히 RRR Mechanism이라고도 하는 모양입니다. {0} 좌표계와 {1} 좌표계는 원점이 같아요...

출처 : Introduction to Robotics 3rd Edition by Craig. Example 3-3

간략하게 표현한 그림이 위에 나타나 있네요. 여기에 대한 Link Parameter는

출처 : Introduction to Robotics 3rd Edition by Craig. Example 3-3

위 그림과 같습니다. 이에 대한 설명이 필요하죠?^^

출처 : Introduction to Robotics 3rd Edition by Craig. 3장

위와 같습니다. 그냥 쉽게 설명하면, a와 alpha는 X축 중심으로 Z축의 이동과 회전... d와 theta는 Z축 중심으로 이동과 회전을 각각 정의하면 됩니다. 

import sympy as sp

th1 = sp.Symbol('th1')
th2 = sp.Symbol('th2')
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 D_x(a):
	return sp.Matrix([[1,0,0,a],[0,1,0,0],[0,0,1,0],[0,0,0,1]])

Trans_0to1 = sp.simplify(RotZ(th1))
Trans_1to2 = sp.simplify(D_x(L1)*RotZ(th2))
Trans_2to3 = sp.simplify(D_x(L2)*RotZ(th3))

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

print('Trans_0to1 = ', Trans_0to1)
print('Trans_0to2 = ', Trans_0to2)
print('Trans_0to3 = ', Trans_0to3)

와 같이 Python의 sympy를 이용해서 Symbolic 연산으로 쉽게 구할 수 있어요^^ 이제 그 결과를 보면

이렇게 됩니다. 이를 수식으로 정리해두면

이 되네요~~~ 그 다음...

이런 결과에 대해...

정리를 해 두었습니다. 이제, {0} 좌표계에서 {1}혹은 {2}, {3}좌표계로의 변환은 모두 구했네요... 이제 얼마전에 보여드린 슬라이더를 적용한 Python 예제[바로가기]대로 꾸며보겠습니다.

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

axcolor = 'lightgoldenrodyellow'

L1 = 1.5
L2 = 1.5

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)
    th2 = dR.conv2Rad(a2)
    th3 = dR.conv2Rad(a3)

    Trans_0to1 = np.r_[np.c_[dR.RotZ(th1), np.zeros(3)], [[0,0,0,1]]]
    Trans_1to2 = np.dot(dR.D_q(L1,0,0), np.r_[np.c_[dR.RotZ(th2), np.zeros(3)], [[0,0,0,1]]])
    Trans_2to3 = np.dot(dR.D_q(L2,0,0), 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)

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

    return ORG_1, ORG_2, ORG_3

ORG_1, ORG_2, ORG_3 = calcORGs(0,0,0)

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)
th2Angle = 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 $', -90.0, 90.0, valinit=0)
s2Angle = Slider(th2Angle, r'$ \theta_2 $', -90.0, 90.0, valinit=0)
s3Angle = Slider(th3Angle, r'$ \theta_3 $', -90.0, 90.0, valinit=0)

dR.drawPointWithAxis(ax, ORG_0, lineStyle='--', vectorLength=1)
dR.drawPointWithAxis(ax, ORG_1, vectorLength=1)
dR.drawVector(ax, ORG_1, ORG_2, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
dR.drawPointWithAxis(ax, ORG_2, vectorLength=1)
dR.drawVector(ax, ORG_2, ORG_3, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
dR.drawPointWithAxis(ax, ORG_3, vectorLength=1)

def update(val):
    th1 = s1Angle.val
    th2 = s2Angle.val
    th3 = s3Angle.val

    ORG_1, ORG_2, ORG_3 = calcORGs(th1, th2, th3)

    ax.cla()

    dR.drawPointWithAxis(ax, ORG_0, lineStyle='--', vectorLength=1)
    dR.drawPointWithAxis(ax, ORG_1, vectorLength=1)
    dR.drawVector(ax, ORG_1, ORG_2, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawPointWithAxis(ax, ORG_2, vectorLength=1)
    dR.drawVector(ax, ORG_2, ORG_3, arrowstyle='-', lineColor='k', proj=False, lineWidth=3)
    dR.drawPointWithAxis(ax, ORG_3, vectorLength=1)

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

s1Angle.on_changed(update)
s2Angle.on_changed(update)
s3Angle.on_changed(update)

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

이렇게 한 번 구현해 보았는데요... 문법이 간결해서 (코드를 잘 짰다는 건 아닙니다. 특히 중복된 코드가 있는 부분이 있는데 어떻게 하면 간결할 것인지 Python 고수님의 조언도 함께 구합니다.ㅠㅠ) 아참 중간에 보면 np.c_나 np.r_이라는 부분이 보이는데요.. 행이나 열을 추가하는 용도로 작성했습니다. 관련예제는 [바로가기]에 있습니다.

아무튼. 처음 실행하면 위와 같은 그림이 됩니다. 밑에 슬라이더로 각 링크의 각도를 조절 할 수 있는거죠^^

뭐.. 이상없이 동작은 하네요^^

이제 좀 더 어려운 예제에 도전해야겠어요.. ㅎㅎ^^