Kinematics 코드 설명

Kinematic class 설명

1. 클래스 생성자 - init

다리 길이 및 몸통의 길이 너비 등의 파라미터 입력을 해주는 곳입니다.

def __init__(self):
        self.l1=50
        self.l2=20
        self.l3=100
        self.l4=100

        self.L = 140
        self.W = 75
        
        #leg iterators. ex. LEG_BACK + LEG_LEFT -> array[2]
        self.LEG_FRONT = 0
        self.LEG_BACK = 2
        self.LEG_LEFT = 0
        self.LEG_RIGHT =1

        #thetas, which will be used on controllers
        self.thetas = np.array([[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0]], dtype=np.float64)

L : 로봇 몸통의 길이

W: 로봇 몸통의 너비

각모터들의 각도들을 모아 둔 thetas 배열이 선언되어 있으며, 이는 4개의 다리 * 3개의 모터에 따라 12개의 값을 갖습니다.

        #leg iterators. ex. LEG_BACK + LEG_LEFT -> array[2]
        self.LEG_FRONT = 0
        self.LEG_BACK = 2
        self.LEG_LEFT = 0
        self.LEG_RIGHT = 1

각 다리에 접근하기 위해 번호가 붙으며,

우리가 사용하는 시스템에서는 그림과 같은 순서로 번호가 매겨 있습니다.

위 코드는 이런 접근을 위한 코드입니다.

예를 들어

Front Left : self.LEG_FRONT + self.LEG_LEFT = 0

Back Right : self.LEG_BACK + self.LEG_RIGHT = 3

이런 식으로 접근이 이루어지는 것이지요

2. bodyIK 함수

원하는 로봇 몸체의 위치/각도를 Input 하면 이에 대한 변환 행렬을 Output 해주는 함수입니다.

def bodyIK(self,omega,phi,psi,xm,ym,zm):
        Rx = np.array([[1,0,0,0],
                    [0,np.cos(omega),-np.sin(omega),0],
                    [0,np.sin(omega),np.cos(omega),0],[0,0,0,1]])
        Ry = np.array([[np.cos(phi),0,np.sin(phi),0],
                    [0,1,0,0],
                    [-np.sin(phi),0,np.cos(phi),0],[0,0,0,1]])
        Rz = np.array([[np.cos(psi),-np.sin(psi),0,0],
                    [np.sin(psi),np.cos(psi),0,0],[0,0,1,0],[0,0,0,1]])
        Rxyz = Rx.dot(Ry.dot(Rz))

        T = np.array([[0,0,0,xm],[0,0,0,ym],[0,0,0,zm],[0,0,0,0]])
        Tm = T+Rxyz

        sHp=np.sin(pi/2)
        cHp=np.cos(pi/2)
        (L,W)=(self.L,self.W)

        return([Tm.dot(np.array([[cHp,0,sHp,L/2],[0,1,0,0],[-sHp,0,cHp,W/2],[0,0,0,1]])),
                Tm.dot(np.array([[cHp,0,sHp,L/2],[0,1,0,0],[-sHp,0,cHp,-W/2],[0,0,0,1]])),
                Tm.dot(np.array([[cHp,0,sHp,-L/2],[0,1,0,0],[-sHp,0,cHp,W/2],[0,0,0,1]])),
                Tm.dot(np.array([[cHp,0,sHp,-L/2],[0,1,0,0],[-sHp,0,cHp,-W/2],[0,0,0,1]]))])

여기서의 변환 행렬은 특정 좌표의 기준이 되는 좌표계를 변경하는데 사용되는 행렬입니다.

코드의 알고리즘 상 보행패턴에 관한 정보는 원점을 기준으로 한 다리 끝점의 위치로 주어지는데요,

하지만 이 보행패턴을 구현하기 위한 각 다리의 모터 각도를 계산하는 Kinematics 과정을 진행하려면 원점을 기준으로 한 위치가 아닌 로봇의 각 어깨 부분을 기준으로 한 다리 끝의 위치를 알아야 합니다.

⇒ 즉 기준 좌표계를 원점에서 각 로봇의 어깨로 옮겨야 하므로 변환 행렬이 필요합니다.

변환 행렬은 회전과 이동 2가지로 나뉘고, 각각은 위 그림과 같이 위치하게 됩니다. ( 제일 하단 행은 무조건 [0 0 0 1]을 갖게 됩니다.)

⇒ 더 자세한 내용이 궁금하다면 로봇공학 동차행렬을 찾아보시길

전체적인 방향은 다음과 같습니다.

좌표계를 원점에서 로봇 몸체 중심으로 바꾸는 행렬과

로봇 몸체에서 각 로봇 어깨로 바꾸는 행렬을 각각 구해서 서로 연결할 것입니다.

원점 ⇒ 로봇 몸체 중심으로 좌표계를 변경 (회전 변환 행렬)

				Rx = np.array([[1,0,0,0],
                    [0,np.cos(omega),-np.sin(omega),0],
                    [0,np.sin(omega),np.cos(omega),0],[0,0,0,1]])
        Ry = np.array([[np.cos(phi),0,np.sin(phi),0],
                    [0,1,0,0],
                    [-np.sin(phi),0,np.cos(phi),0],[0,0,0,1]])
        Rz = np.array([[np.cos(psi),-np.sin(psi),0,0],
                    [np.sin(psi),np.cos(psi),0,0],[0,0,1,0],[0,0,0,1]])
        Rxyz = Rx.dot(Ry.dot(Rz))

몸체의 기울어진 각도를 변환 행렬에 반영하는 부분으로 위의 그림과 같은 3차원 회전 변환 행렬를 사용합니다.

이때 사용하는 3차원 각도 표시 체계는 오일러 각도 이므로(z축 y축 x축 순서로 회전시키는 체계) 곱해지는 회전 변환 행렬의 순서는 Rx Ry Rz 가 됩니다.(계산시 오른쪽 행렬부터 순서대로 곱해짐)

💡

만약 사용하는 각도 체계가 다르다면 코드의 곱해지는 순서를 수정해야 합니다.

원점 ⇒ 로봇 몸체 중심(이동 변환 행렬)

T = np.array([[0,0,0,xm],[0,0,0,ym],[0,0,0,zm],[0,0,0,0]])
Tm = T + Rxyz

몸체의 위치에 관한 정보를 반영해 주고 앞에서 구했던 회전 변환 행렬과 합쳐 최종 변환 행렬을 완성합니다.

최종적으로 Tm 행렬은 곧, 원점 좌표계에서 로봇 몸체의 중심 좌표계로 바꾸어주는 변환 행렬이 됩니다.

로봇 몸체 중심 좌표계 ⇒ 어깨 좌표계

sHp=np.sin(pi/2)
cHp=np.cos(pi/2)
(L,W)=(self.L,self.W)

return([Tm.dot(np.array([[cHp,0,sHp,L/2],[0,1,0,0],[-sHp,0,cHp,W/2],[0,0,0,1]])),
        Tm.dot(np.array([[cHp,0,sHp,L/2],[0,1,0,0],[-sHp,0,cHp,-W/2],[0,0,0,1]])),
        Tm.dot(np.array([[cHp,0,sHp,-L/2],[0,1,0,0],[-sHp,0,cHp,W/2],[0,0,0,1]])),
        Tm.dot(np.array([[cHp,0,sHp,-L/2],[0,1,0,0],[-sHp,0,cHp,-W/2],[0,0,0,1]]))])

앞서 설명했던 회전&이동 변환 행렬을 활용하여 로봇 몸체 중심에서 어깨로의 좌표계 이동을 진행합니다.

회전은 y축(그림상에서 빨강색 축) 90도 회전을 진행하고

이동은 x축(그림상에서 초록색 축)으로 W/2, z축(그림상에서 파랑색 축)으로 L/2 이동을 진행합니다.

이때의 변환 행렬 4개를 계산하여 최종 OUTPUT을 합니다.

legIK

어깨를 중심으로 한 다리 끝점의 좌표가 input 되면 이를 구현하기 위해 각 관절의 모터가 구동해야 할 각도를 Output 해줍니다.

image from : mathworks

def legIK(self,point):
        (x,y,z)=(point[0],point[1],point[2])
        (l1,l2,l3,l4)=(self.l1,self.l2,self.l3,self.l4)
        try:        
            F=sqrt(x**2+y**2-l1**2)
        except ValueError:
            print("Error in legIK with x {} y {} and l1 {}".format(x,y,l1))
            F=l1
        G=F-l2  
        H=sqrt(G**2+z**2)
        theta1=-atan2(y,x)-atan2(F,-l1)
        
        D=(H**2-l3**2-l4**2)/(2*l3*l4)
        try:        
            theta3=acos(D) 
        except ValueError:
            print("Error in legIK with x {} y {} and D {}".format(x,y,D))
            theta3=0
        theta2=atan2(z,G)-atan2(l4*sin(theta3),l3+l4*cos(theta3))
        
        return(theta1,theta2,theta3)

이러한 계산 과정을 역기구학(inverse kinematics)라고 하며, 정확히 우리가 하고자 하는 것을 잘 설명하는 영상이 있어 링크로 대체합니다.

⇒ 영상이 길지 않고 직관적으로 잘 설명되어 있어 꼭 시청하시길 추천드립니다!

https://www.youtube.com/watch?v=IKOGwoJ2HLk

저작자표시 비영리 동일조건

Swiming_Lab

[Spot Dynamixel - 5] Kinematics 코드 설명