로봇의 운동 방정식을 구하고 계산하는 하는 작업들은 매우 복잡한 수식들로 이루어져 있습니다. 그리고, 이러한 연산들은 수치적인 것이 아니라 미지수와 미분, 적분 등 대수적인 연산으로 이루어져 있습니다.

image from : wilselby.com/

• 수치적인 연산은 numpy를 사용하여 해결하며, 대수적인 연산을 위해 파이썬에서는 sympy라는 라이브러리를 많이 사용하고 있습니다.
• 이번 시간에는 sympy의 기본 사용법과 예시들을 통해 앞으로의 코드 구현을 보다 효율적으로 해보겠습니다.

Sympy 기초

• 기본적인 사용법과 예시를 살펴봅시다. (sympy_basic.py를 보시면 됩니다.)

⇒ Jacobian, Matrix 등 더 많은 기능들이 있지만, 이번 예시에서는 위 함수들만을 통해 몇가지 예시들을 구현해보겠습니다.

1. Symbolic Differential vs Numerical Differential

첫번째 예제의 목표는 수치적 미분과 대수적 미분값을 비교해보는 것입니다. 목표 함수로는 아래와 같은 간단한 2차 방정식을 사용하겠습니다.

$f_0 = x^2 + 2x + 1$

• symbolic 미분은 앞선 예시와 같이 간단하게 구할 수 있습니다. - example_diff_sym.py

numerical 미분을 구하는 방법은 여러가지가 있는데요, 고등학교때 배운 미분의 정리에 따라 두가지를 구현해봅시다. - example_diff_num.py

Forward difference

Central difference

⇒ 약간의 차이가 있지만 동일한 4라는 값을 얻게됩니다.

2. Chain Rule 구현

복잡한 함수나 합성함수의 미분 시 chain rule을 사용합니다. chain rule을 통한 미분은 상황에 따라 크게 두가지 종류로 분리됩니다.

• 단일변수 함수의 미분

⇒ 함수 전체가 단일 변수로 구성된 함수의 경우, 일반적인 합성함수의 치환 미분과 같이 계산하며, 아래와 같은 정의를 갖습니다.

$\frac{dy}{dx} = \frac{dy}{du} * \frac{du}{dx}$

• 다변수 함수의 미분 (전미분)

⇒ 예를 들어, $x, y$에 대한 식을 미분할 시, 원하는 변수를 제외한 모든 것을 상수로 간주하는 경우는 편미분이라고 하지요. 하지만, 단일 변수에 대한 미분값이 아니라, 이것이 다변수로 확장될 시 아래와 같은 전미분 식을 사용합니다.

예시를 통해 살펴봅시다. 아래 수식에서 $x$와 $x_d$는 시간에 대한 별도의 변수로 보아야 합니다.

따라서 다변수 함수의 미분, chain rule을 적용해야 합니다.

이제, sympy에서 위의 두 상황을 구현해봅시다. - example_chain_rule.py

3. Projectile under drag force

sympy를 활용하는 예시로 물체의 동역학을 해석하고 이를 수식화, 코드화하는 방법에 대해서 알아보겠습니다.

Euler-Lagrange Method

• 물체에 작용하는 힘을 수식화하기 위한 여러 방법들이 있으며, 첫째로 우리에게 익숙한 뉴턴 Method가 있습니다. F=ma꼴로 물체에 작용하는 모든 힘을 표시하는 방식입니다.

$F = ma$

$\tau = I \alpha$

하지만, 이는 상황에 따른 고려사항이 많아 프로그래밍으로 구현하기에는 다소 번거롭다는 단점이 있습니다. 따라서 이번 시간에는 Euler-Lagrange(E-L) Method라는 것을 배워보겠습니다.

EL Method는 아래와 같은 일련의 정해진 절차를 갖기 때문에 프로그래밍을 통해 구현하기 용이합니다.

1. 강체에 작용하는 모든 힘과, 물체의 물성치(질량, 관성 모멘트, 탄성 계수 등등)를 표현하는 Free body diagram을 완성합니다.
2. 물체의 운동에너지와 위치에너지를 계산하고, 최종적으로 Lagrangian을 구합니다. (L = T - V)
3. Euler-Lagrange Equation의 계산을 통해 EOM 즉, 운동방정식을 구해냅니다.

Projectile Motion

• 자유낙하하는 공의 예시를 통해 EL Method 구현을 같이 진행해봅시다!

• 자유낙하하는 공에는 크게 두가지 힘이 작용합니다.

1. 중력 (물체는 아주 작은 질점으로 가정하고 질량 m을 갖는다고 정의하겠습니다.)
2. drag force - 공기에 의해 발생하는 항력입니다.

• EL Method를 사용하기 위해 물체의 운동에너지와 위치에너지를 계산합니다. 지금은 질점이기 때문에 회전 운동에너지를 고려하지는 않지만, 일반적으로 아래와 같은 값들을 계산해야 할 것입니다.

• 그럼 직교좌표계를 기준으로 운동에너지와 위치에너지, 라그랑지안을 계산해 보겠습니다.

• 아래의 식은 Euler-Lagrange Equation이라는 것으로 운동방정식을 도출하는 중요한 수식입니다. (이번 강의에서 이 수식에 대한 증명을 하지는 않겠습니다.)
• 수식을 전개하는 방법은 바로 이전 step에서 구한 라그랑지안을 state variable에 대해 미분한 것을 좌변, (중력을 제외한) 작용하는 외부 힘을 우변에 적으면 됩니다.

• 현재 우변에 대응하는 작용하는 외부 힘은 drag force로, 이는 속도 제곱에 비례하는 값입니다.

• 2D Space에서 작업하는 만큼 state variable은 x, y 두개이기 떄문에, 각각에 대해 Euler-Lagrange Equation을 계산해야 하고, 이 결과는 아래와 같습니다.

이제 물체의 운동방정식을 계산 완료했으므로, python 코드를 통해 이를 구현해봅시다.

코드 구현

• 앞선 운동 방정식의 결과를 다시 살펴보겠습니다.

⇒ 위 방정식은 x, y축 가속도 즉, 2계 미분을 알려주는 식임에 반해, 우리가 원하는 값은 x, y position자체 입니다. 따라서 위 미분방정식의 적분을 통해 x, y position을 계산해내야 합니다.

⇒ 이러한 적분을 수행하는 방법으로, 우리는 scipy.integrate의 odeint라는 수치적 적분 라이브러리를 사용하고자 합니다.

image from : wikipedia

odeint - Integration of ordinary differential equations

• odeint의 사용법과 매개변수는 다음과 같습니다.

odeint(func, y0, t, args=())

• 예시를 하나 살펴보겠습니다. 이상적인 조건에서 자유 낙하 운동을 하는 물체에는 -g의 중력가속도만 작용하며, 이에 대한 시뮬레이션을 구현해보고 그래프로 속도와 위치를 시각화 해보았습니다. - lec4_dynamics/example_odeint.py

• 0 - 3초의 시간동안 100 sample을 뽑았으며, 초기 조건은 위치 0, 속도 +5로 주어졌습니다. 매개변수로 중력가속도 9.8이 전달되었지요.

t_0, t_end, N = 0, 3, 100
ts = np.linspace(t_0, t_end, N)

result = odeint(free_fall, [0, 5], ts, args=(9.8,))

• 미분방정식을 나타내는 free_fall 함수입니다. 이 함수의 목적은 state vector를 시간에 대해 1계 미분하여 return 하는 것입니다.

def free_fall(z, t, g):
    y, y_d = z
    
    y_dd = -g
    
    return [y_d, y_dd]

그럼, 이제 방금 구한 운동방정식으로 구현을 다시 해보겠습니다.

• state vector가 약간 변경되었을 뿐, 동일한 맥락을 갖고 있음을 알 수 있습니다.

• 실행 결과

cd ./lec4_dynamics
python3 main_projectile.py

4. 다시 구현하는 Free Falling Ball

일전 drag force 운동방정식에 지금까지 배운 내용을 적용해보겠습니다. - lec5_sympy/example_derive_projectile.py

1. sympy를 통해 필요한 symbolic 생성
2. 상태방정식을 세우고 운동에너지와 위치에너지, 라그랑지안을 구하기
3. 라그랑지안을 통해 Euler-Lagrange Equation 계산

• sympy를 통해 필요한 symbolic 생성

2차원 평면에서 회전이 없는 질점의 운동을 고려하겠습니다.

import sympy as sy

x,y = sy.symbols("x y", real=True)
x_d, y_d = sy.symbols("x_d y_d", real=True)
x_dd, y_dd = sy.symbols("x_dd y_dd", real=True)
m,c,g  = sy.symbols('m c g', real=True)

• 상태방정식을 세우고 운동에너지와 위치에너지, 라그랑지안을 구하기

v = sy.sqrt(x_d ** 2 + y_d ** 2)
Fx = -c * x_d * v
Fy = -c * y_d * v

# 라그랑지안 계산
T = 0.5 * m * (x_d ** 2 + y_d ** 2)
V = m * g * y
L = T - V

• 라그랑지안을 통해 Euler-Lagrange Equation 계산

q = [x, y]
q_d = [x_d, y_d]
q_dd = [x_dd, y_dd]
F = [Fx, Fy]

dL_dq_d = []
dt_dL_dq_d = []
dL_dq = []
EOM = []

for i in range(len(q)):
    dL_dq_d.append(sy.diff(L, q_d[i]))
    temp_symbol = 0
    for j in range(len(q)):
        temp_symbol += sy.diff(dL_dq_d[i], q[j]) * q_d[j] + \\
                       sy.diff(dL_dq_d[i], q_d[j]) * q_dd[j]
    
    dt_dL_dq_d.append(temp_symbol)
    dL_dq.append(sy.diff(L, q[i]))
    EOM.append(dt_dL_dq_d[i] - dL_dq[i] - F[i])

⇒ 이렇게 sympy를 통해 얻은 최종 운동방정식은 아래와 같습니다.

Generalized EOMs:
[-c*x_d*sqrt(x_d**2 + y_d**2)/m]
[-c*y_d*sqrt(x_d**2 + y_d**2)/m - g]

앞으로는 이러한 방법으로, 모델 방정식을 먼저 도출하고, 이 수식을 통해 시뮬레이션, 최적화, 제어 등 다양한 부분에 응용해 보겠습니다!