지난 시간, Sympy를 통해 복잡한 운동방정식을 손쉽게 구하는 방법을 배워보았습니다. 앞으로 대부분의 구현에서 운동방정식은 이 방법으로 도출할 것인데요. 이를 연습할 겸, 2 DOF를 갖는 Double pendulum의 운동방정식을 구현하고, 시뮬레이션해봅시다.

개발을 위한 절차는 다음과 같습니다.

1. 각 좌표축 사이의 Homogeneous Matrix 계산
2. 운동에너지와 위치에너지, 라그랑지안 구하기
3. E-L Method를 통해 EOM 계산
4. 시뮬레이션

• 첫째로, 좌표계를 통한 Kinematics 해석을 위해 그림을 작성해보았습니다. 현재 그림과 같이 총 3개의 다른 좌표계가 존재하며 이들 사이 변환 행렬들을 차근차근 구해봅시다.

• $O_0, O_1$ 변환

천장에 매달린 기준으로 좌표계를 잡았기 때문에 joint 각도에 270을 더해야 합니다.

• 기준각을 다른 형태로 해도 괜찮습니다. 대신 다음 과정들에서 모두 통일은 해줘야 할 것입니다.

• $O_1, O_2$ 변환

• $O_0, O_2$ 변환은 Homogeneous Matrix의 특성을 이용하면 손쉽게 구할 수 있습니다.

• 운동에너지와 위치에너지를 구하기 위해선 연결점이 아닌 각 질점의 속도와 y좌표를 구해야 합니다. 지금은 좌표를 구하고 직접 미분하여 속도를 계산했지만, 이후에는 자코비안과 sympy를 사용하여 손쉽게 구할 것입니다.

• 위치에너지와 운동에너지를 통해 라그랑지안을 구한 모습입니다. 현재 기준 좌표계는 x y 직교 좌표계가 아닌 각도 좌표계입니다. (실제 제어시에도 joint angle, velocity, torque를 제어할 것이기 때문입니다.) ⇒ 따라서 모든 위치, 속도, 좌표는 $\theta_1, \theta_2$에 대한 식이 됩니다.

💡 운동에너지를 구할 시, 병진 운동 + 회전 운동에너지를 모두 고려해야 하며 위치에너지를 구할 시에는 각 질점의 y 좌표를 사용합니다.

• 이제, E-L Equation을 구해야 하는데요. 이는 손으로 풀지 않고 일전 구현한 sympy 함수를 사용하고자 합니다! 다만 현재 2 DOF를 가지므로 수식 2개를 얻는다는 점에 유의하세요!

• EOM을 구했다면, 전체 수식을 아래와 같이 간소화할 수 있습니다. theta_1 dotdot과 theta_2 dotdot의 계수와 나머지 항들로 말이지요. 따라서, 전체 EOM 수식을 하나의 행렬꼴로 간소화할 수 있습니다. 이는 코드를 구현하면서 좀 더 살펴보겠습니다.

코드 구현

이번 예시는 크게 두 부분으로 나뉩니다.

1. EOM을 계산하는 sympy 코드 - derive_double_pendulum.py
2. 시뮬레이션을 통한 시각화 코드 - main_double_pendulum.py

• 첫번째, 운동방정식을 도출하는 코드를 실행하면 M, C, G 행렬에 대한 모든 element를 출력해줍니다.

$ python3 derive_double_pendulum.py

M11 =  1.0*I1 + 1.0*I2 + c1**2*m1 + m2*(c2**2 + 2*c2*l1*cos(theta2) + l1**2)
M12 =  1.0*I2 + c2*m2*(c2 + l1*cos(theta2))
M21 =  1.0*I2 + c2*m2*(c2 + l1*cos(theta2))
M22 =  1.0*I2 + c2**2*m2 

C1 =  -c2*l1*m2*theta2_d*(2.0*theta1_d + 1.0*theta2_d)*sin(theta2)
C2 =  c2*l1*m2*theta1_d**2*sin(theta2) 

G1 =  g*(c1*m1*sin(theta1) + m2*(c2*sin(theta1 + theta2) + l1*sin(theta1)))
G2 =  c2*g*m2*sin(theta1 + theta2) 

• 이제 이 값을 그대로 복사하여 Animation 코드에 붙여넣은 뒤 시뮬레이션을 진행하게 됩니다.
  

⇒ 운동방정식을 이끌어내는 코드부터 분석해 보겠습니다.

Derive Double Pendulum

• 필요한 symbolic 변수들을 생성합니다.

import sympy as sy

theta1, theta2 = sy.symbols('theta1 theta2', real=True)
m1, m2 = sy.symbols('m1 m2', real=True)
l1, l2 = sy.symbols('l1 l2', real=True)
c1, c2 = sy.symbols('c1 c2', real=True)
I1, I2 = sy.symbols('I1 I2', real=True)
g = sy.symbols('g', real=True)

• double pendulum의 각 좌표계 사이 변환을 구하고 world frame에서의 joint 위치를 계산합니다.

# link1 frame to world frame
theta1_ = 3*sy.pi/2 + theta1
H_01 = sy.Matrix([
    [sy.cos(theta1_), -sy.sin(theta1_), 0],
    [sy.sin(theta1_),  sy.cos(theta1_), 0],
    [0, 0, 1]
])

# link2 frame to world frame
H_12 = sy.Matrix([
    [sy.cos(theta2), -sy.sin(theta2), l1],
    [sy.sin(theta2),  sy.cos(theta2), 0],
    [0, 0, 1]
])
H_02 = H_01 * H_12

# c1 in world frame
G1 = H_01 * sy.Matrix([c1, 0, 1])
G1_xy = sy.Matrix([G1[0], G1[1]])

# c2 in world frame
G2 = H_02 * sy.Matrix([c2, 0, 1])
G2_xy = sy.Matrix([G2[0], G2[1]])

• 라그랑지안을 도출하기 위해 운동에너지와 위치에너지를 계산합니다. (주의해야 할 점으로 link2의 회전 운동에너지 계산 시 theta1, theta2를 모두 고려해야 합니다!)

# velocity vectors
theta1_d, theta2_d = sy.symbols('theta1_d theta2_d', real=True)
q = sy.Matrix([theta1, theta2])
q_d = sy.Matrix([theta1_d, theta2_d])

v_G1 = G1_xy.jacobian(q) * q_d
v_G2 = G2_xy.jacobian(q) * q_d

# kinetic energy
T1 = 0.5*m1*v_G1.dot(v_G1) + 0.5*I1*theta1_d**2
T2 = 0.5*m2*v_G2.dot(v_G2) + 0.5*I2*(theta1_d+theta2_d)**2
T = T1 + T2

# potential energy
V1 = m1*g*G1[1]
V2 = m2*g*G2[1]
V = V1 + V2

# Lagrangian
L = T - V

• E-L Equation을 계산하며 이때, 이전에 구현한 방식을 사용하면 손쉽게 chain rule을 적용할 수 있습니다.

# Lagrange equation
theta1_dd, theta2_dd = sy.symbols('theta1_dd theta2_dd', real=True)
q_dd = sy.Matrix([theta1_dd, theta2_dd])

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 = 0
    for j in range(len(q)):
        temp += 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)
    dL_dq.append(sy.diff(L, q[i]))
    # 현재 외력이 0이므로 이 두개 항만 있다.
    EOM.append(dt_dL_dq_d[i] - dL_dq[i])

EOM = sy.Matrix([EOM[0],EOM[1]])

계산된 EOM을 한번 출력해봅시다!!

⇒ 자세히 잘펴보면, theta_dd term이 포함된 부분, theta_d term이 포함된 부분과 나머지 항들을 확인할 수 있지요.

• 작성 완료된 EOM에서 행렬꼴을 도출하기 위해 theta_dd term이 포함된 M matrix와 C, G Matrix를 분리해야 하는데요. 이때 트릭이 하나 사용됩니다.
  • 가속도 항에 모두 0을 넣어 A 행렬과 관련된 term을 모두 제거한다. ⇒ b 계산
  • 이 결과의 속도 0을 대입하여 g term만 남긴다. ⇒ G
  • 마지막으로, b - G 가 coriolis term, C가 됩니다.

# M(q)*q_dd + C(q, q_d)*q_d + G(q) -Tau = 0
M = EOM.jacobian(q_dd)
b = EOM.subs([ 
    (theta1_dd, 0),
    (theta2_dd, 0),
])
G = b.subs([ 
    (theta1_d, 0),
    (theta2_d, 0),
])
C = b - G

• 이렇게 계산한 M, C, G 행렬을 print 해봅시다. 실제 print 시에는 sy.simplify를 사용하여 간소화합니다.

print('M11 = ', sy.simplify(M[0,0]))
print('M12 = ', sy.simplify(M[0,1]))
print('M21 = ', sy.simplify(M[1,0]))
print('M22 = ', sy.simplify(M[1,1]),'\\n')

print('C1 = ', sy.simplify(C[0]))
print('C2 = ', sy.simplify(C[1]),'\\n')
print('G1 = ', sy.simplify(G[0]))
print('G2 = ', sy.simplify(G[1]),'\\n')

• 계산된 결과를 이후 simulation 시 그대로 복사 & 붙여넣기 하여 사용할 것이므로 파이썬 문법에 맞게 출력하였습니다.

$ python derive_double_pendulum.py 
M11 =  1.0*I1 + 1.0*I2 + c1**2*m1 + m2*(c2**2 + 2*c2*l1*cos(theta2) + l1**2)
M12 =  1.0*I2 + c2*m2*(c2 + l1*cos(theta2))
M21 =  1.0*I2 + c2*m2*(c2 + l1*cos(theta2))
M22 =  1.0*I2 + c2**2*m2 

C1 =  -c2*l1*m2*theta2_d*(2.0*theta1_d + 1.0*theta2_d)*sin(theta2)
C2 =  c2*l1*m2*theta1_d**2*sin(theta2) 

G1 =  g*(c1*m1*sin(theta1) + m2*(c2*sin(theta1 + theta2) + l1*sin(theta1)))
G2 =  c2*g*m2*sin(theta1 + theta2)

Main Double Pendulum

일전 계산한 M, C, G 매개변수를 통해 운동방정식을 완성했다면, odeint를 통해 시스템을 시뮬레이션할 수 있습니다.

• 예시 실행

python3 main_double_pendulum.py

⇒ 현재 외력이 전혀 존재하지 않는 상황에서 (중력 제외) 수직으로 뻗은 Double pendulum이기 때문에 약간의 틀어짐만 발생해도 매우 격한 움직임을 보입니다.

• 코드를 분석해 볼까요? main에서 odeint를 통해 운동방정식을 전달하고, 0 - 10초 동안의 움직임을 시뮬레이션 해보겠습니다. ⇒ 변화를 보기 위해 omega1에 아주 작은 offset을 설정하였습니다.

if __name__=="__main__":

    params = parameters()

    t = np.linspace(0, 10, 500)
    
    # initlal state
    # [theta1, omega1, theta2, omega2]
    z0 = np.array([np.pi, 0.001, 0, 0])
    all_params = (
        params.m1, params.m2,
        params.I1, params.I2,
        params.c1, params.c2,
        params.l,  params.g
    )
    z = odeint(double_pendulum, z0, t, args=all_params)
    t_interp, z_interp = interpolation(t, z, params)

    animate(t_interp, z_interp, params)

• 이번에는 운동방정식이 구현되어 있는 double_pendulum 함수를 살펴볼까요? ⇒ M 행렬과 이외의 항을 모두 넘겨 b vector를 구성한 뒤, 역행렬을 통해 미분된 state를 계산해냅니다.

def double_pendulum(z0, t, m1, m2, I1, I2, c1, c2, link1, g):
    
    theta1, omega1, theta2, omega2 = z0

    M11 =  1.0*I1 + 1.0*I2 + c1**2*m1 + m2*(c2**2 + 2*c2*link1*cos(theta2) + link1**2)
    M12 =  1.0*I2 + c2*m2*(c2 + link1*cos(theta2))
    M21 =  1.0*I2 + c2*m2*(c2 + link1*cos(theta2))
    M22 =  1.0*I2 + c2**2*m2 

    C1 =  -c2*link1*m2*omega2*(2.0*omega1 + 1.0*omega2)*sin(theta2)
    C2 =  c2*link1*m2*omega1**2*sin(theta2) 

    G1 =  g*(c1*m1*sin(theta1) + m2*(c2*sin(theta1 + theta2) + link1*sin(theta1)))
    G2 =  c2*g*m2*sin(theta1 + theta2)

    A = np.array([[M11, M12], [M21, M22]])
    b = -np.array([[C1 + G1], [C2 + G2]])
    
    x = np.linalg.solve(A, b)
    
    return [omega1, x[0], omega2, x[1]]

현재 중력 이외 어떠한 힘도 작용하고 있지 않지만, 이후 우리는 모터를 통해 토크를 가하고, 원하는 동작을 수행하는 controller, planner, optimizer를 구현해 볼 것입니다. 코드를 꼭 다시 한 번 복기하시고, 저는 다음 강의에서 뵙겠습니다!