로보틱스, 특히 각도를 제어하는 모터와, 실제 직교좌표계 사이 연동이 필요한 시스템에서 Jacobian은 매우 중요하고 반드시 익혀야 하는 개념입니다. 코드 실습과 예시를 통해 자코비안에 대한 이해와 구현을 해봅시다.

• 자코비안의 정의

자코비안의 정의부터 말하자면, 아래와 같이 함수 vector의 다변수 벡터 미분을 표시하는 하나의 방식입니다.

image from : wikipedia

하지만 정의만 보아선 이게 어디에 사용되는지 쉽게 이해할 수 없지요. 지금부터 우리는 Jacobian의 python 구현과 로보틱스에서 사용되는 예시들을 통해 개념을 습득해보겠습니다.

Jacobian sympy 구현

• f라는 함수 벡터의 자코비안을 구하는 식은 아래와 같습니다.

이제 이를 sympy를 통해 구현해봅시다. jacobian_sym.py

• 함수와 변수를 선언하고
• 자코비안의 정의에 따라 각 함수를 변수들로 (편)미분합니다.

• 사실 자코비안은 자주 사용되는 연산이다보니 sympy에서 이미 구현되어 있습니다. sy.Matrix 타입의 변수 내부에서 호출하는 구조를 갖고 있습니다.

z = sy.Matrix([x, y])
J = f.jacobian(z)
print(J)

• 계산된 자코비안식의 변수에 실제 값을 대입하여 결과를 살펴봅시다. jacobian_sym.py

Jacobian numpy 구현

• 자코비안을 좀 더 직관적으로 이해할 수 있는 방법은 numpy로 구현해보는 것입니다. jacobian_num.py

• $f(x + dx, y) - f(x, y)$ 가 dx 항이 되며,
• $f(x, y + dy) - f(x, y)$ 가 dy 항이 됩니다.

계산 결과는 약간의 차이가 있지만, 일전 sympy와 동일한 것도 확인 가능합니다.

다음으로, 자코비안이 로보틱스에서는 어떻게 사용되는지 예시를 통해 살펴보겠습니다.

Application 1 - velocity

로봇의 기준 좌표계는 joint angle 즉, 원 좌표계임에 반해 실제 world에서 우리가 제어하는 좌표계는 x-y 직교좌표계를 갖습니다.

따라서 직교 좌표계에서의 선속도와, 원 좌표계에서의 각속도 사이 변환을 위한 수식이 필요하며, 이 때 바로 속도에 대한 자코비안이 사용됩니다.

결론을 정리해보면 다음과 같습니다.

1. 각도에 대해 표현된 특정 point의 xy좌표가 있을 때,
2. 이를 각도의 벡터로 미분한 뒤 (=자코비안)
3. 이 값에 각도의 1계 미분 벡터(=각속도 벡터)를 곱하면 바로 선속도가 됩니다.

당장 개념적으로 이해가 쉽진 않을 것입니다. 직관적 이해를 위해 일전 double pendulum 예시를 바로 살펴보겠습니다. example_jacobian_double_pendulum.py

⇒ 이전 강의에서 double pendulum의 운동방적식을 구하기 위해 각 joint point, mass point의 xy 좌표를 구한 바 있습니다.

라그랑지안을 구하려면 운동에너지를 구해야 하고, 운동에너지를 구하기 위해선 mass point G1, G2의 선속도가 필요합니다. 방금 전 배운 속도, 각속도 사이의 관계식을 바로 사용해보겠습니다.

• 각도 좌표계 기준인 q vector를 구하고, 질점 G1의 자코비안을 계산합니다.

q = sy.Matrix([theta1, theta2])
# Jacobian of link1 COM
J_G1 = G1_0.jacobian(q)

• 각속도에 대한 state vector를 다시 정의한 뒤, 자코비안에 곱해주면, 이것이 바로 G1의 선속도가 됩니다.

omega1, omega2 = sy.symbols("omega1 omega2", real=True)
q_dot = sy.Matrix([omega1, omega2])

V_G1 = sy.simplify(J_G1 * q_dot)

다시 한 번 왜 이러한 것을 했냐면,

1. 우리가 제어할 수 있는 변수는 모터의 값 (각도)이지만
2. 실제 운동방정식을 세우기 위해서는 운동에너지, 위치에너지를 구해야 하고 이것은 x-y 좌표계를 사용하기 때문에 둘 사이 변환이 필요했습니다.
3. 따라서 각속도와 선속도 변환이 필요하고 이때 자코비안이 사용된 것입니다.

계산 결과는 직접 손으로 미분한 결과와 같습니다. 하지만 앞으로 다룰 더 복잡한 시스템을 대비하여 속도 계산에 대해선 계속해서 자코비안을 도입하겠습니다.

Application 2 - static force

두번째 예시는 수평힘과 토크의 변환입니다.

강체에 외력이 작용하고 있을 때, 평형 상태를 유지하기 위해서는 수평 외력이 평형인 것과 더불어 전체 토크가 평형을 이뤄야 합니다.

따라서, 수평힘이 주어졌을 때, 강체에 작용하는 토크도 0이 되어야 하며 이를 통해 수평힘과 토크 사이의 변환을 구할 수 있습니다.

질점에 작용하는 토크, 수평힘이 평형을 이룬다는 일의 방정식을 통해 아래와 같은 해석이 가능합니다.

이 과정에서 다시 두 좌표계 사이 변환이 이루어지며 이를 보상하는 매개변수가 필요하고, 이것이 바로 자코비안이지요!

아래와 같이 double pendulum에 중력이 작용하는 상황에서 평형 상태를 이루기 위한 토크를 구해보겠습니다.

두 link에 작용하는 중력은 수직 방향이며, 이를 보상하는 각 joint torque를 구하는 것이 우리의 목표입니다. 따라서 point $G_1$, $G_2$에서의 자코비안을 구하고 이를 통해 필요한 토크도 계산할 수 있습니다.

• sympy 코드를 통해 수식을 구현해봅시다. example_jacobian_double_pendulum.py

지금까지는 운동 방정식의 우변이 모두 0이었습니다. 하지만, 이제 중력을 보상하는 만큼의 토크를 가하여 double pendulum이 정상 상태를 유지하도록 시뮬레이션 해보겠습니다.

⇒ 일전 시뮬레이션 코드의 odeint 매개변수가 되는 운동방정식 코드에서 계산한 tau 항을 추가하기만 하면 됩니다.

• 예시를 실행해봅시다.

python3 manipulator_force_jacobian.py

• pendulum이 가만히 정지해 있는 모습을 볼 수 있습니다.
• pendulum에 작용하는 중력과 정확히 동일한 크기의 토크를 가하고 있기 때문에 이렇게 보이는 것이지요.

토크를 가하지 않는다면 제멋대로 움직이는 pendulum을 확인할 수 있습니다.

    A = np.array([[M11, M12], [M21, M22]])
    b = -np.array([[C1 + G1], [C2 + G2]])
    # b = -np.array([[C1 + G1 + tau0], [C2 + G2 + tau1]])

Application 3 - Inverse Kinematics

자코비안의 마지막 예시입니다.

일전 fsolve를 통해 inverse kinematics를 계산하고, 원하는 경로를 따라 이동하도록 하는 작업을 해보았습니다. 이번 시간에는 자코비안을 사용하여 좀 더 안정적인 inverse kinematics를 구현해보겠습니다.

Application 1에서 유도한 자코비안 식을 다시 살펴봅시다.

• 선속도란, xy 좌표계에서의 변화율을 시간으로 나눈 값이고, 각속도란, 극좌표계에서의 변화율을 시간으로 나눈 값이지요.
• 양변의 분모가 되는 시간 변화율을 제거하고, 자코비안의 역행렬을 곱하면, 최종 아래와 같은 식을 얻게 됩니다.

이것이 의미하는 바는 무엇일까요?

• 목표하는 경로, reference와 현재 상태 x가 주어졌을 때, 이들 차이와 자코비안을 곱하면 offset을 보상하기 위해 이동해야 하는 각도를 구할 수 있습니다.
• 어려운 것이 아니라, 결국 xy 좌표계에서의 offset를 joint angle로 변화시키는 것일 뿐입니다.

• 코드 실행을 해보면서 함께 살펴봅시다.

python3 manipulator_inverse_jacobian_circle.py

⇒ 기능 자체는 일전 fsolve를 통한 inverse kinematics와 동일합니다. 하지만 Jacobian을 사용했다는 분명한 차이가 있습니다.

예제 코드는 다음과 같은 개요를 갖습니다.

1. generate_path() : 경로 생성
2. simualtion()
   1. 현재 상태에서의 자코비안과 inverse를 계산합니다.
   2. forward_kinematics를 통해 end_point 좌표를 계산합니다. (실제 로봇 시스템에서는 센서를 사용하게 됩니다.)
   3. 현재 end_point와 목표지점 사이의 offset, 그리고 자코비안을 통해 joint offset을 계산합니다.
   4. joint offset 만큼 pendulum의 joint angle을 제어합니다.

지금 구현한 코드는 일전 fsolve를 통한 inverse kinematics에 비해 다음과 같은 장점을 가집니다.

1. fsolve는 상당한 수치적 계산이 필요한 함수입니다. 물론 자코비안의 역행렬을 구하는 것도 연산이 필요하지만 fsolve에 비하면 훨씬 연산이 적은 방법이라고 할 수 있습니다.
2. double pendulum은 2 자유도를 갖기 때문에 원하는 endpoint를 만족하는 해가 대부분 2개 존재합니다. 그리고 fsolve를 통해 inverse kinematics를 구하게 되면 이 2개의 해 중 어떤 결과를 얻게 될 지 정확히 알기 힘듭니다. 하지만, 자코비안을 사용하면 계산 자체가 이전 상태에 기반하기 때문에 이러한 오류가 없습니다.