지난 시간 좌표변환과 Homogeneous Martix에 대해서 학습해보았습니다. 이번 시간에는 2 link 모델을 통해 이를 실습해보고, Forward Kinematics와 Inverse Kinematics라는 것에 대해 살펴보도록 하겠습니다.

그림과 같은 로봇을 가정해봅시다.

• 이 로봇의 상태는 3개 점의 좌표로 나타내거나, 혹은 2개의 각도로 표기할 수 있습니다. (이를 Degree of Freedom이라고 합니다.)
• 이 각도가 주어지면 Rotation & Translation 연산을 통해 로봇 끝점의 좌표를 알아낼 수 있습니다.
• 우리가 관심있는 것은 로봇 끝점이 특정 좌표에 위치하도록 하는 것입니다. 이를 만족하는 joint각도를 알아내는 것이 목표이지요.

이 상황을 각각 Forward Kinematics / Inverse Kinematics라고 지칭합니다. 우선, 예시 코드의 실행을 통해 실습해볼까요?

코드 실습

• Forward Kinematics 코드 실습

예제 코드를 먼저 실행해보면서 이번 시간 구현할 내용에 대한 감을 익혀봅시다! 프로그램을 실행한 뒤, 원하는 각도를 입력하면 아래와 같이 시각화된 로봇 상태를 확인할 수 있습니다. (Forward Kinematics가 구현되어 있는 것이지요!)

$ cd lec2_kinematics
$ python manipulator_forward.py
theta1 : 0.7
theta2 : 0.

• Inverse Kinematics 코드 실습

이번에는, 원하는 End Effector 좌표를 입력할 시 해당 좌표에 도달하기 위한 joint각도들을 얻게됩니다. (대신, link 길이가 한정되어 있기 때문에 도달할 수 없는 좌표가 있습니다.)

$ python manipulator_inverse.py
=== Type New Ref Points ===
x_ref : 1.0
y_ref : 0.5
theta1 : -0.5139489416444571
theta2 : 1.9551931012905317

Forward Kinematics - 이론

그림과 함께 다시 한 번 정리해보자면 $\theta_1$, $\theta_2$를 통해 O, P, Q 점의 좌표(절대 좌표계 기준)를 계산하는 것이 FK의 목적입니다.

이를 3 step으로 나누어 학습해보겠습니다.

1. 각 joint에 frame 부여
2. 부여된 frame들 사이의 Homogeneous Matrix 구하기
3. 각 link 끝점(O, P, Q)의 world frame 좌표를 계산

• 각 joint에 frame 부여

• 부여된 frame들 사이의 Homogeneous Matrix 구하기

• 각 link 끝점(O, P, Q)의 world frame 좌표를 계산

• 마지막으로, 아래 수식과 같이 Homogeneous Matrix를 통한 계산 결과에서, 마지막 요소인 1을 제거한 결과가 각 link 끝점의 world frame 좌표가 됩니다.

Forward Kinematics - 코드 구현

• 로봇팔 코드를 작성하기 전에, 각도와 수평 운동 offset을 주면 Homogeneous Matrix를 계산해주는 함수를 먼저 구현하였습니다. 이는 오늘 예시에서 계속해서 사용되므로 패키지로서 사용하겠습니다.

• 일전 실행시켰던 코드의 main 함수는 다음과 같이 구성되어 있습니다.

1. 1. 필요한 매개변수들 선언
   2. joint angle theta들을 입력받기
   3. Homogeneous Matrix를 계산
   4. 시각화
      

• 현재 사용되는 매개변수들은 다음과 같습니다.

• plot 함수 안에 Forward Kinematics의 핵심이 구현되어 있으며, 이는 앞서 계산한 수식을 그대로 계산한 것입니다.

$P_0 = H^0_1 * P_1$

$Q_0 = H^0_1 * H^1_2 * Q_2$

Inverse Kinematics - 이론

Inverse Kinematics는 End Effector의 x, y 좌표가 주어졌을 때 해당 끝점을 만족하는 joint angle을 찾는 것이라고 했지요? 일전 end effector의 좌표를 미리 계산해두었으므로 지금 상황에서는 아래와 같은 수식을 푸는 것과 동일합니다.

⇒ 2개의 수식을 통해 2개의 미지수를 계산하는 것이므로 명확한 해가 존재하는 문제입니다.

하지만, sin, cos과 같은 수식으로 손으로 풀 수 없는 형태를 갖고 있습니다. 따라서, 우리는 수치적인 반복 계산을 통해 방정식의 근을 찾아내는 scipy의 fsolve라는 패키지를 사용해보도록 하겠습니다!

Scipy.fsolve

• 예제를 먼저 실행해봅시다.

python3 example_roots.py

⇒ 2차 함수와 x축과의 교점을 계산하여 시각화한 것으로 fsolve는 이렇게 복잡한 수식의 해를 수치적으로 계산할 수 있습니다.

• 코드를 살펴봅시다.

1. fsolve의 첫번째 매개변수는 함수식, 두번째 매개변수는 계산이 시작되는 초기값입니다.
2. fsolve는 첫번째 매개변수인 함수식의 return이 0이 되도록 하는 값을 계산합니다.

• fsolve는 하나의 해를 구해줍니다. 따라서 지금과 같이 해가 2개 이상인 경우, 초기값을 어떻게 설정하느냐에 따라 다른 결과를 얻게 됩니다.

⇒ fsolve를 통해 sin, cos과 같은 비선형 연산에 대해서도 수치적으로 최적해를 구할 수 있습니다.

Inverse Kinematics - 코드 구현

• Inverse Kinemactics의 예시를 다시 실행해볼까요? (원하는 End-Effector 직교 좌표계 위치를 주면 joint 각도를 계산해주고 최종 그림을 볼 수 있습니다.)

python3 manipulator_inverse.py
=== Type New Ref Points ===
x_ref : 0.5
y_ref : 0.0
theta1 : -1.3181160716531397
theta2 : 2.6362321433056852

main 함수를 통해 어떠한 과정이 진행되었는지 살펴봅시다.

1. 사용자로부터 end effector 좌표를 전달받습니다.
2. fsolve를 통해 해당 좌표를 만족하는 joint value를 계산합니다.
3. joint value를 통해 다시 forward kinematics를 계산하여 그림을 도출합니다.

• fsolve의 매개변수인 함수 inverse_kinematics는 다음과 같은 기능을 수행합니다.

1. 1. 임의의 초기값으로 일단 FK를 진행합니다.
   2. FK의 결과인 End Effector의 좌표와 원하는 직교 좌표계 사이의 차이를 return 합니다.

Inverse Kinematics - Trajectory Following

• Inverse Kinematics를 사용하면 다음과 같은 기능 구현도 가능합니다.

python3 traj_manipulator_inverse.py

(1, 0.5)를 중심으로 하는 원의 경로를 생성하여 IK를 실행한 것이지요!

phi  = np.arange(0,2*np.pi,0.2)
x_ref_list = 1  + 0.5*np.cos(phi)
y_ref_list = 0.5+ 0.5*np.sin(phi)

실제 로봇팔이 이러한 식으로 궤적을 그리는 것은 아니지만, Inverse Kinematics를 모른다면 로봇팔의 제어를 수행하기 어렵습니다. 이번 시간에는 double link 모델을 사용하였지만 이제 triple, quadra 로봇팔에 대해서도 어떻게 작업해야 할지 감이 잡히셨지요? 🙂