24강 3D Manipulator

이번 시간에는 3차원상의 Manipulator를 다루는 시뮬레이션을 제작해볼 것입니다.
  • 2차원상의 double pendulum을 표현할 시, 각 joint 사이 좌표변환을 homogeneous martix를 사용해 표현했 던 것처럼, 이번 3차원 Manipulator 또한 4 * 4 행렬을 사용하여 homogeneous martix를 구성해줘야 하는데요. 이를 위해 사용하는 특별한 방법, “DH method”에 대해 배워보고 코드 구현도 함께 해보겠습니다.


  • 강의에 앞서, manipulator는 link 사이 결합 상태에 따라 아래와 같이 Open, Closed manipulator로 나뉩니다. 이번 시간에는 open manipulator에 대해서만 학습해보겠습니다.


  • 일반적으로 Manipulator는 N개의 joint와 N개의 Link를 가진 시스템으로 정의할 수 있습니다. 더불어 가장 마지막 link 끝단에 해당하는 부분은 end effect라고 불리며, 이 지점에 gripper가 장착되어 실제 물체를 잡는 과정에 사용됩니다.


  • 따라서, 우리가 원하는 좌표, end-effector의 정확한 위치를 알아내기 위해선 각 joint들 사이의 Homogeneous Matrix를 모두 구해야 하며, 이는 Forward Kinematics라고 불리는 과정이었습니다.


  • Homogeneous Matrix를 구하기 위해선 joint사이 각도 3가지, 수평 offset 3가지를 모두 알아야 할 것입니다. 따라서 총 6개의 매개변수를 알아야 하는 것이지요. 더불어 manipulator가 회전 이동을 해야 하기 때문에 이들 6개의 매개변수 중에 적어도 하나는 자유도를 갖는 변수여야 합니다.


Denavit–Hartenberg parameters

Homogeneous Matrix를 구하기 위해 모든 joint 마다 6개의 매개변수를 모두 구하는 작업은 매우 힘듭니다. 이번 시간 우리는 이를 보다 쉽게 구할 수 있게 해주는 방법인 Denavit–Hartenberg parameters라는 것을 실습해보려 합니다.
  • DH parameter는 6개가 아닌, 4개의 변수를 통해 3차원 상의 모든 homogeneous martix를 표현합니다. (link들끼리 결합되어있다는 속성에서 2개의 변수가 불필요해지기 때문입니다.)

이렇게 될 수 있는 이유는 4개의 매개변수를 설정할 시 특별한 조건을 적용했기 때문인데요. 설명만 읽어선 이해하기 쉽지 않은 개념이라 직접 실습을 통해 터득해보겠습니다.


DH method를 적용하는 순서는 다음과 같습니다.

  1. manipulator의 모든 Joint에 z축을 설정합니다. (일반적으로 회전하는 축을 z축으로 두게 됩니다.)
  2. 시작점이 되는 base frame의 x, y 축을 설정합니다. ( x축이 정해지면 y축은 자동으로 정해지며, x축은 임의의 축으로 해도 무관합니다.)
  3. 나머지 frame들에 순서대로 x축을 설정합니다. 현재 z축은 모두 정해졌으므로 이에 기반하여 x축을 할당하는 것이며, 아래의 법칙에 따라 전후 z축의 상황을 고려하여 z축을 설정합니다. ( $z_{i-1}, z_{i}$ 를 기준으로 $x_{i}$ 를 설정하는 것입니다.)
    1. $z_{i-1}, z_{i}$ 가 완전히 다른 평면에 위치한다면, 둘과 모두 수직인 unique한 축을 $x_{i}$ 로 설정합니다.
    2. $z_{i-1}, z_{i}$ 가 평행한다면, 둘 모두에 수직인 무한한 축이 가능하며, $x_{i}$ 는 이전 frame의 원점 O를 지나도록 설정합니다. (나중에 살펴 보겠지만 이렇게 설정해야 최대한 많은 변수를 0으로 할 수 있습니다. )
    3. $z_{i-1}, z_{i}$ 가 한 점에서 만난다면, 가능한 두가지 경우가 있습니다. 이때는 $x_{i}$ 를 이전 축인 $x_{i-1}$에 평행한 방향으로 설정합니다.
  4. End Effector의 xyz 축을 설정하며 End Effector의 z축을 바로 이전 frame의 z축과 평행하도록 하는 것이 이후 계산 시 유리합니다.
  5. 모든 joint에 x축과 y축이 설정되었으므로 cross product를 통해 y축도 구할 수 있습니다. 이에 기반하여 이제 DH table을 작성합니다. (아래 그림을 참고해주세요!)
    1. $a$ ⇒ $x_{i}$ 기준 $z_{i}$, $z_{i-1}$ 사이의 거리
    2. $\alpha$ ⇒ $x_{i}$ 기준 $z_{i}$, $z_{i-1}$ 사이의 각도
    3. $d$ ⇒ $z_{i-1}$ 기준 $x_{i}$ , $x_{i-1}$ 사이의 거리
    4. $\theta$ ⇒ $z_{i-1}$ 기준 $x_{i}$ , $x_{i-1}$ 사이의 각도
  6. DH Table을 모두 구했다면, 모든 joint 끝점의 각도와 위치를 알아낼 수 있게 됩니다.


이제, 예시를 통해 DH Table을 작성해봅시다!

Example1 - 2D double pendulum


  • 두개의 joint와 두개의 link, 그리고 end effector를 갖는 2D double pendulum의 DH table을 작성해봅시다.

  1. manipulator의 모든 Joint에 z축을 설정합니다. ⇒ 2D space에서 z축은 튀어나오는 방향이므로 손쉽게 설정 가능합니다.
  2. 시작점이 되는 base frame의 x, y 축을 설정합니다. ⇒ 이미 설정되어 있으므로 생략하겠습니다.
  3. 나머지 frame들에 순서대로 x축을 설정합니다. ⇒ 현재 모든 z축들이 평행하기 때문에 case b를 적용하여 이전 joint의 원점을 지나도록 새로운 x축들을 설정하면 됩니다.
  4. End Effector의 xyz 축을 설정합니다. ⇒ z축은 이전 joint와 평행하도록, x축은 이전 joint의 원점을 지나도록 설정하였습니다.
  5. 모든 joint들에 대해 xyz 축을 그려보았으며 결과는 아래 그림과 같습니다!

이제, 위 그림을 토대로 DH table을 작성해봅시다.

  • $a$ ⇒ $x_{i}$ 기준 $z_{i}$, $z_{i-1}$ 사이의 거리
  • $\alpha$ ⇒ $x_{i}$ 기준 $z_{i}$, $z_{i-1}$ 사이의 각도
  • $d$ ⇒ $z_{i-1}$ 기준 $x_{i}$ , $x_{i-1}$ 사이의 거리
  • $\theta$ ⇒ $z_{i-1}$ 기준 $x_{i}$ , $x_{i-1}$ 사이의 각도

Link $a_i$ $\alpha_i$ $d_i$ $\theta_i$
1 $l_1$ 0 0 $\theta_1$
2 $l_2$ 0 0 $\theta_2$

  • DH table만 있다면, 모든 joint 사이 좌표변환을 자유자재로 다룰 수 있게됩니다. end-effector의 좌표를 계산하기 위해 link0 ⇒ link1 ⇒ link2 변환을 계산해봅시다. homogeneous martix 계산 결과에서 end-effector의 rotation과 position을 쉽게 도출할 수 있게 됩니다.


Example2 - 1R2P 3 link manipulator

두번째 예시로, 1개의 회전하는 revolute joint와 2개의 수평 이동을 하는 prismatic joint를 갖는 1R2P의 3 link manipulator의 DH table을 작성해 보겠습니다. 앞선 예시와 같이 절차를 나누어 차근차근 진행해 보도록 하겠습니다.

  1. manipulator의 모든 Joint에 z축을 설정합니다. ⇒ joint의 회전축 혹은 수평운동하는 방향을 z축으로 설정합니다.
  2. 시작점이 되는 base frame의 x, y 축을 설정합니다. ⇒ x축을 면에서 튀어나오는 방향으로 설정하였습니다.
  3. 나머지 frame들에 순서대로 x축을 설정합니다.
    1. z0와 z1이 평행하기 때문에, 두 축에 수직하는 무수히 많은 x1이 가능합니다. 하지만 DH table에서 $\theta$를 0으로 만들어주는 축, x0와 평행한 축을 x1으로 설정하였습니다.
    2. z1과 z2가 수직하기 때문에, 두 축에 모두 수직하는 두가지 경우가 가능합니다. 하지만 이번에도 DH table에서 $\theta$를 0으로 만들어주는 축, x1과 평행한 축을 x2로 설정하였습니다.
  4. End Effector의 xyz 축을 설정합니다. ⇒ z축은 이전 joint와 평행하도록, x축은 DH table에서 $\theta$를 0으로 만들어주는 축으로 설정하였습니다.
  5. 이번에도, 모든 joint들에 대해 xyz 축을 그려보았으며 결과는 아래 그림과 같습니다.


그림을 토대로 DH table을 작성해봅시다.

  • $a$ ⇒ $x_{i}$ 기준 $z_{i}$, $z_{i-1}$ 사이의 거리
  • $\alpha$ ⇒ $x_{i}$ 기준 $z_{i}$, $z_{i-1}$ 사이의 각도
  • $d$ ⇒ $z_{i-1}$ 기준 $x_{i}$ , $x_{i-1}$ 사이의 거리
  • $\theta$ ⇒ $z_{i-1}$ 기준 $x_{i}$ , $x_{i-1}$ 사이의 각도


Link $a_i$ $\alpha_i$ $d_i$ $\theta_i$
1 0 0 $l_1$ $\theta_1$

  • z0, z1는 완전 일치하므로 이들 사이 거리는 0이 되며 각도도 0도가 됩니다.
  • x0, x1 사이 거리는 l1이며, x0 to x1 각도는 고정된 것이 아니라, joint1의 angle임에 유의합니다.


Link $a_i$ $\alpha_i$ $d_i$ $\theta_i$
2 0 270° $d_2$ 0

  • z1, z2는 한 점에서 만나기 때문에 거리는 0, 각도는 z1 to z2 270°가 됩니다.
  • x1, x2는 평행하며 거리는 prismatic joint의 offset, 각도는 0도가 됩니다.


Link $a_i$ $\alpha_i$ $d_i$ $\theta_i$
3 0 0 $d_3$ 0

  • z2, z3는 완전 일치하기 때문에 거리는 0, 각도도 0이 됩니다.
  • x2, x3는 평행하며 거리는 prismatic joint의 offset, 각도는 0도가 됩니다.


완성된 DH table입니다!

Link $a_i$ $\alpha_i$ $d_i$ $\theta_i$
1 0 0 $l_1$ $\theta_1$
2 0 270° $d_2$ 0
3 0 0 $d_3$ 0

Example3 - 3R 3 link manipulator

코드 구현 전 마지막 예시로 3개의 revolute joint를 갖는 3R 3 link manipulator의 DH table을 작성해봅시다. ( 이는 실제 로봇팔에서 가장 많이 사용되는 형태입니다.)

  1. manipulator의 모든 Joint에 z축을 설정합니다. ⇒ joint의 회전축 혹은 수평운동하는 방향을 z축으로 설정합니다.
  2. 시작점이 되는 base frame의 x, y 축을 설정합니다. ⇒ 설정할 수 있는 x축이 무수히 많지만 우측 방향으로 설정해보겠습니다.
  3. 나머지 frame들에 순서대로 x축을 설정합니다.
    1. ⇒ z0와 z1이 한 점에서 만나기 때문에, 두 축에 수직하는 두개의 x1이 가능합니다. 하지만 DH table에서 $\theta$를 0으로 만들어주는 축, x0와 평행한 축을 x1으로 설정하였습니다.
    2. ⇒ z1과 z2가 평행하기 때문에, 두 축에 모두 수직하는 무수히 많은 경우가 가능합니다. 하지만 이번에도 DH table에서 $\theta$를 0으로 만들어주는 축, x1과 일치하는 축을 x2로 설정하였습니다.
  4. End Effector의 xyz 축을 설정합니다. ⇒ z축은 이전 joint와 평행하도록, x축은 DH table에서 $\theta$를 0으로 만들어주는 축으로 설정하였습니다.
  5. 모든 joint들에 대해 xyz 축을 그려보았으며 결과는 아래 그림과 같습니다. (z축과 x축만을 그려두었지만 y축은 이 둘에 따라 자동으로 결정됩니다.)


이번에도, DH table을 채워봅시다!
  • $a$ ⇒ $x_{i}$ 기준 $z_{i}$, $z_{i-1}$ 사이의 거리
  • $\alpha$ ⇒ $x_{i}$ 기준 $z_{i}$, $z_{i-1}$ 사이의 각도
  • $d$ ⇒ $z_{i-1}$ 기준 $x_{i}$ , $x_{i-1}$ 사이의 거리
  • $\theta$ ⇒ $z_{i-1}$ 기준 $x_{i}$ , $x_{i-1}$ 사이의 각도


Link $a_i$ $\alpha_i$ $d_i$ $\theta_i$
1 0 90° $l_1$ $\theta_1$

  • z0, z1는 한 점에서 교차하므로 이들 사이 거리는 0, 각도는 90°가 됩니다.
  • x0, x1 사이 거리는 l1이며, x0 to x1 각도는 고정된 것이 아니라, joint1의 angle이 됩니다.


Link $a_i$ $\alpha_i$ $d_i$ $\theta_i$
2 $l_2$ 0 0 $\theta_2$

  • z1, z2는 평행하며, 이들 사이 거리는 l2, 각도는 0°가 됩니다.
  • x1, x2는 완전 일치하기 때문에 이들 사이 거리는 0, x1 to x2 각도는 joint2의 angle이 됩니다.


Link $a_i$ $\alpha_i$ $d_i$ $\theta_i$
3 $l_3$ 0 0 $\theta_3$

  • z2, z3는 평행하며, 이들 사이 거리는 l3, 각도는 0°가 됩니다.
  • x2, x3는 완전 일치하기 때문에 이들 사이 거리는 0, x2 to x3 각도는 joint3의 angle이 됩니다.


DH table을 완성시켜봅시다.

Link $a_i$ $\alpha_i$ $d_i$ $\theta_i$
1 0 90° $l_1$ $\theta_1$
2 $l_2$ 0 0 $\theta_2$
3 $l_3$ 0 0 $\theta_3$


실제 코드 구현 시, sympy를 통해 아래와 같은 martix를 미리 작성해두고 DH parameter만 대입하면 손쉽게 rotation과 position을 구할 수 있을 것입니다.

코드 구현

첫번째 코드는 DH parameter를 sympy로 구현하는 예시입니다. 아래 수식에 맞추어 행렬을 정의하고 4개의 DH parameter를 대입한 결과를 계산합니다.
  • 행렬 정의와 곱셈만 하면 되기 때문에 매우 간단합니다.


  • 계산 결과는 다음과 같습니다.

앞으로의 코드들은 일전 구한 DH table을 사용하여 Forward Kinematics를 계산하고, matplotlib를 통해 plot하는 것입니다.


DH parameter를 사용한다면, 2D/3D/1R2P/3R 전부 동일한 형식으로 Forward Kinematics를 계산할 수 있으며, 이 뼈대가 되는 코드를 먼저 살펴보고 각 상황들에 대해 구현해보겠습니다.


  • $a, \alpha, d, \theta$를 input으로 받아 Homogeneous Matrix H를 output으로 반환하는 함수, DH2Matrix를 구현합니다.


  • Parameter 클래스 내부에 joint 수 만큼 DH parameter 4세트를 정의합니다. 지금은 double pendulum이기 때문에 4세트 두 쌍, 총 8개의 매개변수가 정의되었습니다.

💡 코드 구현 시 참고를 위해 DH table과 좌표계 그림을 다시 정리하면 아래와 같습니다.

Link $a_i$ $\alpha_i$ $d_i$ $\theta_i$
1 $l_1$ 0 0 $\theta_1$
2 $l_2$ 0 0 $\theta_2$


  • 다음으로 main 함수입니다. Parameter 클래스에서 필요한 매개변수를 모두 추출한 다음, 일전 구현한 DH2Matrix 함수로 전달하여 Homogeneous Matrix를 얻습니다. Homogeneous Matrix로부터 각 joint position을 추출한 다음, 최종 plot 함수에게 전달합니다.


  • plot 함수는 각 joint point들을 연결하여 link를 시각화합니다.


  • 코드의 실행 결과는 다음과 같습니다. 현재 joint angle을 모두 0으로 설정했기 때문에 축 쳐진 모습입니다.


  • DH parameter에서 $\theta$ 부분을 수정해보고 결과를 다시 확인해봅시다.

  • 다음 예시는 1R2P 3D manipulator입니다. 이번에도 DH table과 그림을 참고하여 코드를 분석하겠습니다.

Link $a_i$ $\alpha_i$ $d_i$ $\theta_i$
1 0 0 $l_1$ $\theta_1$
2 0 270° $d_2$ 0
3 0 0 $d_3$ 0


  • 3 joint 시스템이기 때문에 3쌍의 DH parameter가 필요하며, 1개의 revolute joint angle 2개의 prismatic joint distance가 필요합니다.

코드의 대부분은 이전과 동일하기 때문에 추가된 부분만 별도 설명하겠습니다.
  • DH set이 하나 더 추가됨에 따라 main 함수의 구현에 변화가 있습니다.


  • 3D manipulator가 됨에 따라 plot 함수에도 변화가 있습니다.


  • 코드 실행 결과는 다음과 같습니다. revolute, prismatic joint 모두 원점 상태의 그림입니다.


  • DH parameter에서 joint parameter를 원하는 값으로 수정한 뒤, 예시를 다시 실행해보세요!


마지막 3R manipulator의 구현은 joint 속성만 바뀐 것으로 앞선 예시와 맥락은 완전 동일합니다. 때문에 설명은 생략하지만, 직접 구현해보면서 복습해보는 것을 추천합니다.

Complete and Continue