9강 Passive Walker

Passive Walker란 아래 그림과 같이 모터의 직접적인 힘이 아닌, 중력에 의한 가속과 바닥과의 충격량으로 동작하는 시스템을 말합니다. 일반적인 모터 제어보다 훨씬 효율적이고, 안정적인 시스템을 갖고 있어 보행을 연구할 시 가장 기초가 되는 모델입니다.

image from : wikipedia

이번 시간에는, 이러한 passive dynamic walker model을 해석해보고, 지난 시간 배운 hybrid system을 적용하여 직접 시뮬레이션까지 진행해보겠습니다. 더불어 Double Pendulum과 Passive Walker의 비교를 통해 "Underactuated Robot"에 대해서도 간단히 제시드리고자 합니다.


Passive Walker

Passive Walker는 바닥에 고정된 Stance Leg와 움직이는 Swing Leg로 구성되며, 다리 입장에서 이동하는 상태인 swing phase과 바닥을 짚은 strike 이후 고정되며 stance phase가 반복되는 Hybrid System을 갖습니다.

  1. Leg Swing
  2. Foot Strike
  3. Leg Stance

⇒ 1, 2에는 각기 다른 dynamics가 적용되며 따라서 이는 hybrid system이라고 말할 수 있습니다.

Walker Modeling


Walker의 운동방정식을 세우기 위해 joint와 link 질점을 정의하겠습니다.

Point Description
P Hip joint
C1 Leg1 endpoint
C2 Leg2 endpoint
G1 Leg1 COM
G2 Leg2 COM
theta1 Leg1과 수직선 사이 각도
theta2 Leg1으로부터 Leg2 사이 각도 ( 이 각도에 주의합니다. )
etc g, M, m, I, l, c, gamma…


  • 좌표계를 설정하고, Homogeneous 연산을 통해 각 질점과 endpoint, hip joint의 위치 / 속도를 계산합니다. 유의할 점으로 Passive Walker는 계속해서 이동하는 로봇이기 때문에 바닥과 접하는 점 $C_1$ 의 좌표가 계속해서 변화합니다. 이러한 상황을 “Floating Base”라고 부른답니다.

추가적인 유의점은 다음과 같습니다.

  • 편의를 위해 C1의 좌표를 X축 기준으로 설정하였습니다. 이에 따라 회전 행렬식의 요소에도 차이가 생김을 확인합시다.
  • $x_1$ ⇒ $x_2$ 사이 각도 변환은 $180 - \theta_2$이지만, 방향이 시계방향이므로 - 부호가 붙어 $\theta_2 - 180$이 됩니다.


다음으로 Hip joint P와 Leg endpoint, 각 leg 질량 중심의 좌표를 계산해봅시다.


E-L Equation

E-L Equation을 계산하기 위해 운동에너지와 위치에너지를 구해보겠습니다.
  • 운동에너지

Homogeneous 연산을 통해 각 point의 위치를 얻어냈기 때문에 속도는 자코비안을 통해 손쉽게 계산할 수 있습니다. (leg2의 회전운동에너지를 구할 때 사용되는 각도는 $\theta_1 + \theta_2$ 가 됨에 유의합니다.)

  • 위치에너지

현재 walker는 기울어진 경사면을 따라 움직입니다. 따라서 y 좌표를 얻어내기 위해 경사각도 고려해줘야 합니다. 경사각 $r$만큼의 회전 변환을 반대로 적용해야 하므로 방정식에 $R^T$가 곱해진 모습을 확인할 수 있습니다.

이 시점에서 상기해보면, 현재 walker는 foot-strike 시점에 다른 운동방정식을 갖는 hybrid system이었습니다. 따라서 swing / strike 시 각기 다른 운동방정식이 적용됩니다.
  • swing EOM ⇒ 외력 0
  • strike EOM ⇒ impact map


leg2가 바닥과 충돌하는 시점, 각 endpoint에 작용하는 외력을 F1, F2라고 해보겠습니다. 이 힘을 현재 state(theta1, theta2)의 static force로 변환하기 위해서, 이전 Jacobian 시간 때 학습한 바와 같이 접촉점의 Jacobian을 구하고 이것의 transpose를 가해지는 힘과 곱해줘야 합니다.

수학적으로 살짝 이야기하자면 이는 제약조건을 가진 상황의 최대값을 구하는 라그랑주 승수법이 적용된 형태입니다. 라그랑주 계수 (multiplier)가 Jacobian이 되는 것이지요.

그럼, 위 운동방정식에서 $F_{C_1}$, $F_{C_2}$는 어떻게 구할 수 있을까요??

Impact Map

물리 시간에 배웠던 충격량에 대해 복습해봅시다. 순간적인 속도 변화에 의한 충격량은 곧 작용하는 시간과 힘의 곱으로 표현할 수 있었지요.

image from : 로고스 학원


Walker의 운동방정식 양변에 시간에 대한 순간 적분을 적용하면 $F dt$ 항을 이끌어낼 수 있으며 이를 충격량 I로 치환할 수 있게 됩니다.

Strike가 발생하는 짧은 순간, 운동방정식에 어떠한 변화가 생기는지 살펴봅시다.

  • C1에서는 충격이 발생하지 않기 때문에 작용하는 힘은 0이 됩니다.
  • 매우 짧은 시간에 대한 적분을 계산하기 때문에 중력과 코리올리 힘은 0으로 가정할 수 있습니다.
  • 마지막으로 C2의 순간 속도는 0이라고 가정할 수 있습니다.

남은 수식들을 정리하고 속도에 대한 제약조건까지 모두 적용하면 아래와 같은 2개의 수식을 얻을 수 있으며, 이를 행렬 형태로 나타낼 수 있습니다.

6개의 미지수 $(x, y, \theta_1, \theta_2, I_{C_x}, I_{C_y})$, 6개의 방정식을 가지므로 Full Rank(미지수와 방정식의 수가 일치하는 경우)를 만족하여 모든 요소를 계산할 수 있게 됩니다.

추가되는 항에 대해 제약조건을 추가하여 전체 시스템을 해석 가능하게 하는 이러한 기법은 Impact Map이라고 지칭하며, 바닥과 상호작용하는 보행 로봇의 해석에 있어 필수적으로 사용되는 기법입니다.

  • 출처 : High speed trot-running: Implementation of a hierarchical controller using proprioceptive impedance control on the MIT Cheetah


마지막으로, walker의 switch 조건에 대해 살펴봅시다. foot strike → swing이 발생하기 위한 조건으로 이는 Swing leg가 바닥면과 닿는 시점이 됩니다.

switch가 발생한 직후, 새로운 state를 부여하여 다음 swing을 준비시킵니다.


Stability of Walking & Poincaré map

지금까지 학습한 내용을 바탕으로 구현된 passive walker 코드를 실행해보겠습니다.
cd lec9_passive_walker
python3 main_walker_naive_guess.py


Step이 계속될수록 보폭이 작아지면서 불안정해지는 모습을 볼 수 있는데요. 보행의 안정성을 보장하기 위해 어떠한 추가작업이 필요할지, 수학적인 접근을 해보겠습니다.

⇒ 현재 walker는 동일한 보행을 반복 실행하고 있습니다. 매번 보행이 반복될 때마다 발생하는 오차가 누적되어 점차 불안정해지고 있는 상황이지요.

이를 수식적으로 나타내봅시다. 보폭을 거치기 전과 거친 후의 state를 나타내는 함수를 $F(x)$ 라고 하였을 시 보폭을 거친 뒤의 상태 $x_{i+1} = F(x_i)$로 나타낼 수 있습니다. (이는 Poincaré map이라고도 불립니다.)

보폭, F(x)를 거칠 때마다 발생하는 차이, dx는 곧 Jacobian이 됩니다. 이 Jacobian을 최소화하는 것이 목적이지요.

더불어 Jacobian의 eigen value를 통해 stable 여부를 확인할 수 있습니다. (모든 eigenvalue가 1보다 작으면 시스템이 stable하다는 뜻이 됩니다.)

현재 상황은 leg의 각도를 직접적으로 제어하지 않고, 오로지 경사에 의한 중력, 초기 속도를 통해 보행하게 됩니다. 각도를 제어하는 제어기는 이후 강의에서 다시 다루어보겠습니다.

코드 구현

복잡한 운동방정식을 계산하는 sympy code와 시뮬레이션 코드, 2종류의 구현을 살펴보겠습니다.

sympy 코드를 통해 운동 방정식을 자동 계산하고, 이 결과를 numpy를 활용하는 시뮬레이션 코드에 대입하여 최종 시뮬레이션을 완성시키게 됩니다.

derive_walker.py는 sympy를 사용하는 코드로 아래와 같은 수식들을 구현합니다.

  1. kinematics
  2. E-L Method
  3. single_stance
  4. Heelstrike

main_walker.py는 계산된 운동방정식을 바탕으로 시뮬레이션을 진행합니다. solve_ivp를 통해 terminal condition 이전까지 미분방정식의 해를 구해줍니다.

  • n_steps
  • one_step
    • single_stance
    • footstrike


derive_walker.py

Passive Walker의 kinematics를 복기하며 코드를 살펴봅시다.

각 leg의 발끝 점, C1 C2 / 각 leg의 무게중심 G1, G2 / Hip joint H 위치와 joint의 위치를 계산해야 합니다.

  • 모델 전체가 이동하는 floating base이기 떄문에 C1의 좌표가 $(x, y, 1)$ 이 된다는 점에 유의합니다.
  • 각도 기준에 따른 축 사이의 회전 변환 계산에 유의합니다.

  • 다음으로 운동에너지와 위치에너지를 계산하여 lagrangian를 도출합니다. 운동에너지를 구하기 위해 질점의 jacobian을 사용하여 속도를 계산하였습니다.

  • 위치에너지 계산을 위해 질점의 y좌표를 추출합니다. 이때 주의해야 할 점은 현재 로봇이 경사에 위치하므로 이를 보상하는 회전 행렬을 곱해줘야 한다는 것입니다.


일전 구해둔 위치에너지와 운동에너지 공식을 통해 Lagrangian을 계산합니다.

이제, E-L Method를 통해 운동방적식을 계산합시다. 지금까지 이 계산을 수도 없이 반복한 만큼 이전 구현한 코드를 그대로 사용하겠습니다.

Passive Walker는 Swing-Contact을 반복하며 각 상태 전환 시마다 다른 시스템을 갖는 hybrid system이라고 언급한 바 있습니다. 두 종류의 시스템 dynamics 중 swing을 single_stance라고 이름 붙였습니다.

외력이 발생하지 않는 single stance 상황에서는 EOM의 우변이 0이 될 것입니다. 이 상황에서 $Ax=b$를 만족시키는 Matrix A, Vector b를 계산합시다.

# Ax = b
A_ss = EOM.jacobian(q_dd)
b_ss = []

for i in range(len(q_dd)):
    b_ss.append(-1 * EOM[i].subs([(ax, 0), (ay, 0), (alpha1, 0), (alpha2, 0)]))

leg가 바닥과 충돌하는 순간은 Heel strike라고 지칭하겠습니다. Heelstrike 시 impact map을 구하기 위해 M, z, J가 필요합니다. 이때 사용되는 Jacobian, J는 충돌하는 point(C1 혹은 C2가 되겠지요)의 좌표를 state로 미분한 Jacobian이 됩니다.

Impact Map의 실질적인 계산은 simulation 코드에서 numpy를 사용하도록 하겠으며, 지금은 이를 위해 필요한 행렬인 M과 J만 구해두겠습니다.

C2_xy = sy.Matrix([ C2[0], C2[1] ]) 
J_C2 = C2_xy.jacobian(q)

# A_n_hs => 4 * 4
# J_n_sw => 2 * 4
A_n_hs = A_ss.subs([ (theta1, theta1_n), (theta2, theta2_n) ])
J_n_sw = J_C2.subs([ (theta1, theta1_n), (theta2, theta2_n) ])

시뮬레이션 코드 분석

실질적인 시뮬레이션을 구현해둔 코드를 분석해보겠습니다. 일단 Poincare Map은 고려하지 않고 모델 구현에 집중하겠습니다.

walker의 modeling state는 theta1, theta2로 두었습니다. (실제 모델과 유사하게 하고 싶다면 바닥과의 마찰, 수직항력 등을 고려해야 하기에 state로 x, y도 필요하지만 이번 실습에서는 간소화를 위해 생략하겠습니다.)

theta1, omega1, theta2, omega2 = 0.2, -0.25, -0.4, 0.2

t0 = 0
step_size = 10
z0 = np.array([theta1, omega1, theta2, omega2])

one_step

swing ⇒ strike ⇒ swing ⇒ strike가 반복되기 때문에 swing ⇒ strike의 one step을 구현하는 함수를 구현해두었습니다.

solve_ivp를 통해 heel strike 전까지의 state를 계산하며 strike 조건을 collision이라는 event로 지정하여 solve_ivp에게 전달합니다.

single_stance로 사용되는 운동방정식은 일전 구현한 sympy 코드의 결과에서 그대로 복사 & 붙여넣기 합니다. 다만, derive 코드에서 사용한 state는 x y theta1 theta2 였었던 반면, 현재는 state로 theta1, theta2만을 사용하고 있습니다. 이에 따라 약간의 변형이 필요합니다.

새롭게 운동방정식을 구할 필요는 없으며, A matrix, b vector에서 마지막 두개의 요소만 추출하여 사용하면 됩니다. 😊

충돌 조건을 구현한 함수 collision은 다음과 같습니다.

이때, 주의해야 할 점이 있는데요, 일전 이론 시간에 살펴본 collision 조건은 사실 두 다리가 마주치는 상황에서도 발생합니다. 따라서 theta1이 일정 각도보다 크다는 조건을 추가합니다.

def collision(t, z, M, m, I, l, c, g, gam):

    output = 1
    theta1, omega1, theta2, omega2 = z

    if (theta1 > -0.05):
        output = 1
    else:
        output = 2 * theta1 + theta2

    return output

다음으로 foot strike입니다. 충돌 전 state를 기반으로 Impact map을 적용하여 충돌 후 각속도를 구해내는 것이 목적이지요.

	z_minus = np.array(sol.y_events[-1][0,:])
	t_minus = sol.t_events[-1][0]
	
	z_plus = footstrike(t_minus, z_minus, params)
	
	t[-1] = t_minus
	z[-1] = z_plus
	
	return z, t

footstrike함수 내부에는 derive 코드의 마지막 M, J를 사용하여 직접 이 연산을 구현해야 합니다.

M, J matrix를 구한 뒤 np.block을 통해 Ax = b 꼴을 구성합니다. (참고로, 현재 사용중인 state가 theta에 대한 요소만 갖고 있기 떄문에 충돌 전 vx, vy는 명시적으로 0으로 넣어주었습니다.)

    A_fs = np.array([[A11, A12, A13, A14], [A21, A22, A23, A24], [A31, A32, A33, A34], [A41, A42, A43, A44]])

    M_fs = np.block([
        [A_fs, -np.transpose(J_fs) ], 
        [J_fs, np.zeros((2,2))] 
    ])

    b_fs = np.block([
        A_fs.dot([0, 0, omega1_n, omega2_n]), 0, 0
    ])

    # x_hs => [vx(+), vy(+), omega1(+), omega2(+) ]
    x_hs = np.linalg.inv(M_fs).dot(b_fs)

Impact map을 통해 충돌 후 속도를 계산한 뒤, switch 조건을 적용하고 return합니다.

    omega1_plus = x_hs[2] + x_hs[3]
    omega2_plus = -x_hs[3]

    return [theta1_plus, omega1_plus, theta2_plus, omega2_plus]

one_step의 마지막으로 z와 t행렬의 마지막 성분을 footstrike 결과로 치환합니다. 다음 swing의 초기조건을 준비하는 것입니다.

   z_plus = footstrike(t_minus, z_minus, params)

    t[-1] = t_minus
    z[-1] = z_plus

    return z, t

n_steps

one_step을 반복하는 n_steps 함수입니다.

구현 측면에서 설명을 덧붙이자면, animation의 용이를 위해 Hip position xh, yh를 미리 계산하여 추가하고 있습니다. 따라서 zz_temp에는 state + hip point 들이 저장됩니다.

yh는 단순히 다리 길이와 $cos(\theta_1)$의 곱셈이 되지만, xh는 약간의 트릭을 사용하고 있습니다. 이는 그림을 통해 설명해보겠습니다.

  • 코드로 구현된 모습은 다음과 같습니다.

one_step을 특정 횟수 동안 반복하는 것이 n_steps의 주된 목적이었습니다. 이에 따라 swing ⇒ strike를 거친 뒤, 다음 loop를 위해 state를 초기화해야 하며, 이때 zz_temp[-1]에는 스위칭이 일어난 이후이기 때문에 다음 state의 hip joint x 좌표는 zz_temp[-2,4]를 사용합니다.

		for i in range(step_size):
		    ...
		    theta1, omega1, theta2, omega2 = z_temp[-1,0:4]
		    z0 = np.array([theta1, omega1, theta2, omega2])
		    t0 = t_temp[-1]
		
		    # one_step에서 zz_temp[-1] 스위칭이 일어나기 때문에 [-2] 사용
		    xh_start = zz_temp[-2,4]
		
    return z, t

Visualization

n_steps 결과는 animation, plot을 통해 시각화됩니다. animation 로직은 간단히 다음과 같습니다.
  1. interpolation
  2. 바닥면 그리기
  3. H, C1, C2, G1, G2 좌표 추출
  4. 추출된 점들로 직선 그리기
  5. 화면 이동시키기
  6. remove

Hip joint position을 알고 있기 때문에 model 전체를 animation하는 것은 어렵지 않습니다. 다만 로봇이 움직임에 따라 화면도 같이 움직이게 하는 코드 구현은 잠시 살펴보고 가겠습니다.

    min_xh = min(z[:,4]) 
    max_xh = max(z[:,4])

    dist_travelled = max_xh - min_xh;
    camera_rate = dist_travelled/len(t_interp);

    window_xmin = -1*l; window_xmax = 1*l;
    window_ymin = -0.1; window_ymax = 1.1*l;

    R1 = np.array([min_xh-l,0])
    R2 = np.array([max_xh+l,0])

    # 바닥은 처음에 다 그려버린다.
    ramp, = plt.plot([R1[0], R2[0]],[R1[1], R2[1]],linewidth=5, color='black')

animation이 종료된 이후 theta, theta_dot, xh, yh를 plotting 하면서 코드가 마무리됩니다.

Stable Condition

Poincaré map에서 제자리로 되돌아오는 즉, 만족하는 안정적인 보행을 만족하는 state를 찾아봅시다.

one_step 이전과 이후 state에 변화가 없다면 안정적인 보행을 하고 있다고 말할 수 있습니다. 이를 위한 initial state를 찾기 위해 fsolve를 사용하여 one_step이전, 이후의 차이를 0으로 만드는 initial state를 계산해냅니다.

z_star = fsolve(fixedpt, z0, params)
...
def fixedpt(z0, params):

    z, t = one_step(z0, 0, params)

    return z[-1,0]-z0[0], z[-1,1]-z0[1], z[-1,2]-z0[2], z[-1,3]-z0[3]

fixedpt의 결과가 얼마나 안정적인지 수치적으로 표현하는 방법에 대해 설명하였지요?

⇒ state 변화율인 Jacobian의 eigenvalue를 사용하여 판단할 수 있었습니다.

image from : wiki pedia

다만, one_step의 Jacobian을 대수적으로 구하기는 너무 복잡하므로 수치적인 Jacobian 함수를 구현하였습니다. (참고로 이렇게 미분값을 근사화하는 방법은 최적화에도 자주 사용됩니다.)

구현 측면에서 말해보자면, Jacobian의 정의에 따라 각 state별 미소 변화량을 적용한 결과로 Matrix를 채워주었습니다.

partial_jacobian을 통해 도출한 Jacobian의 eigenvalue를 계산하여 stability를 판단합니다.

아래 예시를 보면, 모든 eigenvalue의 절대값이 1보다 작은 stable한 시스템이 만족되었습니다.
J_star = partial_jacobian(z_star, params)
eig_val, eig_vec = np.linalg.eig(J_star)
print(f"eigVal {eig_val}")
print(f"eigVec {eig_vec}")
print(f"max(abs(eigVal)) : {max(np.abs(eig_val))}")

# example
eigVal [-3.16662218e+00 -7.95961269e-02 -2.73839757e-10  7.21287672e-02]

이번 강의 중 언급된 수학적인 내용을 좀 더 공부하고 싶다면 아래 링크들을 확인해보세요!


Underactuated Robotics

double pendulum과 passive dynamic walker까지 배운 현 상황에서, 비슷하지만 또 다른 두 예제를 분석해보도록 하겠습니다.

double pendulum은 $\theta_1, \theta_2$의 2 자유도를 갖는 시스템으로, 각각의 joint에 모터가 부착되어 제어할 수 있는 예시였습니다.

잠시 double pendulum을 180도 뒤집어서 볼까요? 그리고 passive dynamic walker와 비교해봅시다!

그림과 같이 double pendulum과 passive dynamic walker는 기구학적으로 동일한 시스템입니다. 하지만 double pendulum은 2 자유도 state, 2 자유도 control을 갖는 반면, passive dynamic walker는 2 자유도 state, 1 자유도 control을 갖는 시스템이지요!

이렇게 control dof가 system의 dof보다 작은 경우를 “Underactuated System”이라고 부르며 보행 로봇과 대부분의 항공, 수중 로봇들은 underactuated system을 갖습니다.

혹은 일반적인 Fully-actuated 로봇들도 장애물, 모터의 최대, 최소 토크 제약에 따라 underactuated system로 변형되기도 합니다. 따라서 로봇을 개발함에 있어 underactuated system은 피할 수 없는 상황이라고 말할 수 있지요.

underactuated system은 control 자유도가 Fully-actuated에 비해 낮기 때문에 제어 신호를 계산함에 있어 추가 작업을 필요로 합니다. (Fully-actuated의 경우 Feedback Equivalent를 만족하기 때문에 제어 신호의 계산이 보다 용이합니다.)

수학적 모델과 물리에 기반한 제어와 강화학습/딥러닝 기반 접근법 등 underactuated system을 다루는 여러 방법론들이 존재합니다. 딥러닝 기반 접근법에 대한 연구가 활발히 이루어지고 있지만, 많은 학습 데이터를 필요로하고, 결과에 대해 명확한 해석을 내리기 어렵다는 단점을 갖고 있습니다.

따라서 이번 강의들에서는 어떠한 기법을 사용해도 가장 기본이 되는 모델링과 제어에 대해 학습하고 이를 python으로 구현하면서 시뮬레이션과 결과 분석을 계속 이어나가도록 하겠습니다! 😊

Complete and Continue