제3부 동역학

3.13 부유기저 다물체 동역학과 중심 동역학(centroidal dynamics)

3.13.1 이 장에서 배우는 것 / 선수 지식 / 특히 중요한 독자

이 장은 고정된 기저가 없는 로봇(휴머노이드·사족 로봇·비행 중인 몸체)의 동역학을 다룬다. 몸통에 자유 비행 관절 6자유도를 두고 관절 12개를 단 다물체를 대상으로 질량행렬, 바이어스 힘, 질량 중심, 중심 운동량 행렬 $A_G$, 중심 동역학 항등식, 비행 중 운동량 보존, 운동량이 0인 순환 관절 운동이 만드는 몸통 회전, 접지점(ZMP)과 중심 동역학의 관계를 순수 파이썬으로 계산하고 Pinocchio·MuJoCo·SciPy와 같은 입력에서 비교한다. 접촉력의 모형(상보성·마찰 원뿔·충격)은 3.12, 공간 벡터 대수와 CRBA·ABA는 3.5, 뉴턴-오일러 재귀는 3.2, 동역학 라이브러리는 3.6, 전신 제어는 12.7, ZMP 보행과 LIPM은 14.15, 동적 보행은 14.16, MPC 기반 다리 이동은 14.17, 접촉 계획은 14.22, 민첩 이동은 14.23과 이어진다.

배우는 것:

• 부유기저의 표현 — 자유 비행 관절(SE(3)), 설정 $n_q=n_j+7$(쿼터니언)·속도 $n_v=n_j+6$, 몸체 속도 좌표, 구동되지 않는 6자유도.
• 운동방정식의 분할 — $M_{bb}$·$M_{bj}$·$M_{jj}$ 블록, 첫 여섯 행이 외력의 합이라는 성질.
• 중심 운동량 행렬 $A_G$ — $h_G=A_Gv$, 선형 행 $=m\,J_{\mathrm{com}}$, 계수 6, 몸통 블록이 합성 강체 관성.
• 중심 동역학 — $\dot l=\sum_if_i+mg$, $\dot k=\sum_i(p_i-c)\times f_i$. 관절 토크는 나타나지 않는다.
• 비행과 순환 운동 — 총 운동량 보존, 속도 구속 $V_0=-A_b^{-1}A_j\dot q$, 닫힌 관절 경로의 순 회전.
• ZMP와의 관계 — $(\ddot c,\dot k)$에서 구하는 영 모멘트점과 LIPM이 되는 조건.

선수 지식: 1.7(SO(3)·SE(3)의 지수 사상), 3.1(공간 관성), 3.2(RNEA), 3.5(CRBA·공간 벡터 대수), 3.12(접촉력).

특히 중요한 독자: 휴머노이드·사족 로봇의 전신 제어·보행 계획을 구현하는 개발자, 동역학 라이브러리의 부유기저 규약(좌표 순서·몸체 대 세계 속도)을 이식하는 연구자, 낙상·충돌 안전 해석에서 총 운동량을 다루는 안전 엔지니어.

출처 방침. 기준 시점은 2026-11-05이다. 직접 읽은 자료: Modern Robotics 사전 인쇄본(2017-05-03) 8.2.2절(공간 운동량 $P_b=G_bV_b$ (8.34)와 뉴턴-오일러 식 $F_b=G_b\dot V_b-[\mathrm{ad}_{V_b}]^{T}G_bV_b$), Pinocchio 4.1.0의 함수 설명(ccrba·computeCentroidalMomentum·computeCentroidalMomentumTimeVariation·computeCentroidalMap·centerOfMass·crba·rnea·aba·integrate), MuJoCo 문서의 자유 관절 정의와 mj_subtreeVel(subtree_linvel·subtree_angmom). 서지만 확인한 것(본문은 읽지 못했다): Orin·Goswami(2008), Orin·Goswami·Lee(2013), Wensing·Orin(2016), Lee·Goswami(2012), Dai·Valenzuela·Tedrake(2014), Herzog·Schaal·Righetti(2016), Ponton 외(2016), Kajita 외(2001), Vukobratović·Borovac(2004), Popović·Goswami·Herr(2005), Kane·Scher(1969), Montgomery(1993), Featherstone(2008). 이들 문헌의 식과 알고리즘(CCRBA·$O(N)$ 계산)은 이 장이 직접 유도하고 계산으로 검증했으며 원문과의 일치는 확인하지 못했다 (확인 필요). 모형(질량 46.2 kg, 관절 12개)의 모든 값은 이 장이 정한 예시이며 어떤 제품의 제원도 아니다. 수치는 순수 파이썬 계산이다.

3.13.2 개념 — 기저가 움직이는 로봇은 외력이 첫 여섯 행으로만 들어온다

결론부터 적으면 다음과 같다. 부유기저 로봇은 구동되지 않는 6자유도(기저)와 구동되는 관절 $n_j$개로 이루어진다. 접촉력과 중력은 로봇 전체에 작용하는 외력이며 운동방정식의 첫 여섯 행에만 나타나고 관절 토크는 그 행에 들어가지 않는다. 따라서 관절 토크만으로는 로봇 전체의 합력과 합토크를 바꿀 수 없고 접촉력이 있어야 몸 전체가 가속된다. 이 성질을 질량 중심 기준으로 쓴 것이 중심 동역학이다. 중심 운동량 $h_G=(k,l)$은 질량 중심에서 본 전체 각운동량과 선운동량이며 관절 속도에서 선형 사상 $h_G=A_G(q)v$로 얻고, 시간 변화율은 접촉 렌치와 중력 렌치의 합이다. 이 여섯 식은 몸 전체의 자유도(휴머노이드는 수십 개)와 무관하게 성립하므로 보행·점프의 계획과 제어에서 축약 모형으로 사용된다.

이 장의 계산 결과를 미리 적으면 다음과 같다. ① 순수 파이썬 CRBA·바이어스 힘·질량 중심·$A_G$가 Pinocchio와 $1.7\times10^{-13}$ 이내, MuJoCo의 관절 질량행렬·비행 가속도·운동량과 $7.4\times10^{-13}$ 이내에서 같다. ② $\dot h_G$의 5점 차분이 (몸통 렌치를 질량 중심으로 옮긴 값 + 중력 렌치)와 $7.7\times10^{-9}$ 이내로 같다. ③ 관절 토크가 무작위인 비행 1 s에서 몸통이 $102^\circ$ 회전하는 동안 각운동량의 변화가 RK4($\Delta t=2.5$ ms)에서 $2.0\times10^{-7}$이고 간격에 대해 4차로 수렴한다. ④ 운동량이 0인 채 왼팔 어깨 두 관절을 반지름 $0.4$ rad 원 경로로 1회 순환시키면 몸통이 $9.14^\circ$ 회전하고(대부분 수직축), 이 값이 운동량 구속 적분과 전체 동역학에서 $2\times10^{-10}$ rad 안에서 같다. ⑤ ZMP를 접촉 렌치로 구한 값과 $(\ddot c,\dot k)$ 식으로 구한 값이 $4.2\times10^{-11}$ m 안에서 같고, 몸통 피치 각가속도 $5$ rad/s²에서 LIPM 식과 $4.6$ cm 어긋난다.

3.13.3 원리

3.13.3.1 부유기저의 설정과 속도

기저(몸통)의 자세는 $(R_0,p_0)\in SE(3)$이고 설정은 이것과 관절각 $q_j\in\mathbb R^{n_j}$이다. 자세를 쿼터니언으로 표현하면 $n_q=n_j+7$, 속도는 6차원이므로 $n_v=n_j+6$이다. 두 차원이 다르므로 $\dot q\ne v$이며 설정의 갱신은 지수 사상 $R_0\leftarrow R_0\exp([\omega_b]_\times\Delta t)$, $p_0\leftarrow p_0+R_0v_b\Delta t$로 한다(Pinocchio의 integrate가 같은 구조의 표준 적분기라고 설명한다). 이 장의 기저 속도 $V_0=(\omega_b,v_b)$는 몸통 프레임에서 본 트위스트이고 본문의 규약(BK-05)은 각속도가 먼저이다. Pinocchio는 속도에서 선속도가 먼저이고 설정이 위치와 쿼터니언 $(x,y,z,w)$ 순서이며, MuJoCo는 설정의 쿼터니언이 $(w,x,y,z)$ 순서이다. 이 장의 라이브러리 대조(풀이 예제 ⑤)에서는 Pinocchio의 속도를 $(v,\omega)$ 몸체 좌표로, MuJoCo의 속도를 병진 세계 좌표·회전 몸체 좌표로 두었을 때 모든 양이 일치했다. 오일러각 6개를 가상 관절로 사용하는 표현은 자세 특이점이 있어 이 장은 시험하지 않았다.

3.13.3.2 운동방정식의 분할과 구동 불가 자유도

접촉이 있는 부유기저의 운동방정식은 기저 블록과 관절 블록으로 나뉜다. $S=(0\ \ I)$이면 구동은 $S^{T}\tau$이다.

$$\begin{pmatrix}M_{bb}&M_{bj}\\ M_{jb}&M_{jj}\end{pmatrix}\begin{pmatrix}\dot V_0\\ \ddot q_j\end{pmatrix}+\begin{pmatrix}h_b\\ h_j\end{pmatrix}=\begin{pmatrix}0\\ \tau\end{pmatrix}+\sum_kJ_k^{T}f_k.$$

첫 여섯 행은 $M_{bb}\dot V_0+M_{bj}\ddot q_j+h_b=\sum_kJ_{k,b}^{T}f_k$이며 $J_{k,b}^{T}f_k$는 접촉 $k$의 렌치를 기저 프레임으로 옮긴 것이다. 좌변은 몸 전체의 운동량 변화율에서 중력을 뺀 것(RNEA의 기저 렌치)이다. 접촉력이 정해지면 아래 $n_j$행에서 $\tau=M_{jb}\dot V_0+M_{jj}\ddot q_j+h_j-\sum_kJ_{k,j}^{T}f_k$로 관절 토크가 결정된다. 접촉력 자체가 접촉 모형(3.12)과 마찰 원뿔 제약을 따르므로 전신 제어(12.7)는 접촉력을 변수로 두고 위 식을 등식 제약으로 둔다. 구속이 있는 일반 형태의 라그랑주 승수는 3.11이 다룬다.

3.13.3.3 합성 강체 관성과 CRBA

기저 블록 $M_{bb}$는 현재 자세에서 로봇 전체를 하나의 강체로 본 합성 강체 관성 $I_0^{c}$이다. 모든 물체의 관성을 부모 쪽으로 합성해($I_p\leftarrow I_p+{}^{i}X_p^{T}I_i\,{}^{i}X_p$) 올라가면 얻고, 관절 열은 $F_j=I_j^{c}S_j$를 조상 프레임으로 옮기며 $M_{ij}=S_i^{T}\,{}^{i}X^{*}_{j}F_j$로 채운다(Featherstone의 합성 강체 알고리즘, 서지만 확인). 이 장의 구현에서 $M_{bb}$의 선형 $3\times3$ 블록은 총질량 $m=46.2$ kg의 $mI$이다. $I_0^{c}$가 양정부호이므로 $M_{bb}$는 가역이고, 아래의 $A_b$도 가역이다. 바이어스 힘 $h(q,v)$는 가속도 0의 RNEA이며 중력은 기저의 공간 가속도 $(0,-R_0^{T}g)$로 넣는다. 공간 속도·가속도·힘의 재귀는 MR 8.2.2절의 식 $F=G\dot V-[\mathrm{ad}_V]^{T}GV$와 같다.

3.13.3.4 질량 중심과 중심 운동량 행렬

질량 중심 $c$에 원점을 두고 세계 좌표와 축이 같은 프레임 $G$에서 본 전체 공간 운동량이 중심 운동량 $h_G=(k,l)$이다.

$$h_G=\sum_i{}^{G}X^{*}_{i}\,I_iV_i=A_G(q)\,v,\qquad A_G={}^{G}X^{*}_{0}\,M[0\!:\!6,\,:\,],\qquad {}^{G}X^{*}_{0}=\begin{pmatrix}R_0&[p_0-c]_\times R_0\\ 0&R_0\end{pmatrix}.$$

운동량이 질량행렬의 첫 여섯 행에 담기는 것은 운동 에너지의 기저 속도 미분이 운동량이기 때문이다($\partial T/\partial V_0=h_0$). 성질은 세 가지이다. 선형 운동량은 $l=m\dot c$이므로 $A_G$의 선형 행은 $m\,J_{\mathrm{com}}$이다. $A_G$는 $6\times n_v$이며 계수가 6이다(기저 블록이 가역). $h_G=0$을 만족하는 속도의 집합은 관절 속도 $\dot q_j$에서 기저 속도가 유일하게 정해지는 $n_j$차원이다. 각운동량 $k$는 질량 중심에 대한 값이며 모든 링크의 $I_i\omega_i+m_i(r_i-c)\times\dot r_i$의 합이다. Orin·Goswami(2008)가 이 행렬을 중심 운동량 행렬이라 부르며 구조와 성질을 정리했고 이를 효율적으로 계산하는 알고리즘(CCRBA)과 $O(N)$ 개선이 후속 문헌(Orin 외 2013, Wensing·Orin 2016)에 있다(서지만 확인). Pinocchio의 ccrba가 이 행렬과 중심 합성 강체 관성을 계산한다.

3.13.3.5 중심 동역학

중심 동역학은 중심 운동량의 시간 변화율이다. 몸 전체에 작용하는 외력은 접촉 렌치와 중력이며 중력은 질량 중심에 작용하므로 중력의 모멘트는 질량 중심에 대해 0이다.

$$\dot h_G=\begin{pmatrix}\dot k\\ \dot l\end{pmatrix}=\begin{pmatrix}\sum_i\bigl[(p_i-c)\times f_i+\tau_i\bigr]\\ \sum_if_i+mg\end{pmatrix},\qquad \dot h_G=A_G\dot v+\dot A_Gv.$$

여기서 $g=(0,0,-9.81)$ m/s²는 중력 가속도 벡터이다. 접촉점 $p_i$의 위치가 정해지면 $\dot h_G$는 접촉력 $f_i$에 선형이다. 관절 토크와 관절 가속도는 $A_G\dot v$ 항으로만 나타나 접촉력이 정해진 뒤 하위 $n_j$행에서 구한다(3.13.3.2). 접촉력은 $f_{i,n}\ge0$과 마찰 원뿔 $\lVert f_{i,t}\rVert\le\mu f_{i,n}$(3.12)을 만족해야 하고, 이 부등식과 접촉 순서가 계획의 어려움이다(14.22). 중심 동역학을 완전한 기구학과 함께 계획하거나 접촉력 최적화를 구조화하는 연구(Dai 외 2014, Herzog 외 2016, Ponton 외 2016)가 있으며 이 장은 원문을 읽지 못했다.

3.13.3.6 비행과 순환 운동 — 운동량 보존과 홀로노미

접촉이 없으면 $\dot k=0$이고 $\dot l=mg$이다. 관절 토크는 내력이므로 몸통이 얼마나 회전하든 $k$는 일정하다. 초기 $h_G=0$이면 $A_bV_0+A_j\dot q_j=0$에서 기저 속도가 관절 속도로 정해진다.

$$V_0=-A_b^{-1}A_j\,\dot q_j,\qquad A_G=\bigl(A_b\ \ A_j\bigr).$$

이 일차 속도 구속은 일반적으로 적분되지 않는다(비홀로노미). 관절각이 닫힌 경로를 지나 처음 값으로 돌아와도 몸통 자세는 처음과 같지 않다. 낙하하는 고양이가 회전 없이 자세를 바꾸는 현상을 이 구조로 설명하는 문헌이 Kane·Scher(1969)와 Montgomery(1993)이다(서지만 확인). 이 장은 구속식과 전체 동역학(3.13.3.2의 첫 여섯 행)의 두 경로로 몸통 회전을 계산해 일치를 확인한다.

3.13.3.7 ZMP와 중심 동역학

지면($z=0$)에서 접촉 렌치의 수평 모멘트가 0인 점 $P=(P_x,P_y,0)$를 ZMP라 한다. 질량 중심 기준 접촉 렌치를 $(\tau_G,f)$로 두면 $\tau_G=(P-c)\times f+(0,0,\tau_z)$이고, 중력 가속도의 크기를 $g=9.81$ m/s²로 두어 $f=m(\ddot c+g\hat z)$, $\tau_G=\dot k$를 대입하면 다음이 나온다.

$$P_x=c_x-\frac{c_z\,\ddot c_x}{\ddot c_z+g}-\frac{\dot k_y}{m(\ddot c_z+g)},\qquad P_y=c_y-\frac{c_z\,\ddot c_y}{\ddot c_z+g}+\frac{\dot k_x}{m(\ddot c_z+g)}.$$

질량 중심 높이가 일정($\ddot c_z=0$)하고 각운동량이 변하지 않으면($\dot k=0$) $P_x=c_x-(c_z/g)\ddot c_x$가 되며 이것이 선형 도립진자 모형(LIPM, Kajita 외 2001; Popović 외 2005와 Vukobratović·Borovac 2004는 ZMP와 관련 지점들을 다룬다. 모두 서지만 확인)의 식이다. LIPM이 근사하는 항은 $\ddot c_z$와 $\dot k$이다. 보행의 ZMP 제어와 LIPM 계획은 14.15이 다룬다.

3.13.4 알고리즘

CRBA(q):    Ic_i ← I_i;  i = N..1:  Ic_parent += X_{i←p}ᵀ Ic_i X_{i←p}
            M_bb ← Ic_0;  관절 j:  F ← Ic_j S_j,  조상 k로 올라가며 M_kj ← S_kᵀ F,  F ← X*_{p←k} F,  기저 행 M_bj ← F
바이어스(q, v):   A_0 ← (0, −R_0ᵀ g);  i = 1..N:  V_i ← X V_p + S qd,  A_i ← X A_p + ad(V_i) S qd
                  F_i ← I_i A_i − ad(V_i)ᵀ I_i V_i;  i = N..1:  F_p += X* F_i;  h ← (F_0, Sᵀ F_i)
A_G(q):     c ← Σ m_i c_i / m;   A_G ← ^G X*_0 M[0:6, :]
비행 한 걸음:   a ← M⁻¹ ([0, τ] − h),   (p, R, q, V, qd) ← RK4,  R ← 그람-슈미트(R)
순환 운동:    V_0 ← −A_b⁻¹ A_j qd,   R ← R exp([ω]× dt)

3.13.5 풀이 예제 — 부유기저 다물체의 질량행렬·중심 운동량·비행·홀로노미·ZMP

결론부터 적으면 다음과 같다. ① 순수 파이썬 CRBA·바이어스 힘·$A_G$가 Pinocchio와 MuJoCo에 $10^{-13}$ 이하에서 같다. ② 중심 동역학 항등식이 $7.7\times10^{-9}$ 이내(5점 차분의 한계)에서 성립한다. ③ 비행 1 s의 운동량과 에너지 보존 오차가 간격 $\Delta t$에 대해 4차로 줄어든다. ④ 운동량 0 순환 운동의 순 회전이 반지름 제곱에 비례한다($0.1$ rad에서 $0.589^\circ$, $0.8$ rad에서 $32.39^\circ$). ⑤ ZMP 식이 접촉 렌치와 $4.2\times10^{-11}$ m 안에서 같다.

구현. Xm·Xf·sp_inertia·ad·crba·bias·com_ag·deriv·rk4. 아래 코드는 1.1의 matmul·transpose·solve_lu와 1.7의 hat·exp_so3을 사용한다. 모형(make_tree), 링크 합 검사, 홀로노미 적분, ZMP 식, 그림은 스크립트에 있다.

from math import sqrt, sin, cos, pi
I3 = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]

def mv(A, x):
    return [sum(a * b for a, b in zip(r, x)) for r in A]

def add(a, b):
    return [x + y for x, y in zip(a, b)]

def sc(k, a):
    return [k * x for x in a]

def Xm(R, p):
    """운동 변환 ^A X_B, 트위스트 (w, v) 순서"""
    pR = matmul(hat(p), R)
    return [R[i] + [0.0] * 3 for i in range(3)] + \
           [pR[i] + R[i] for i in range(3)]

def Xf(R, p):
    """렌치 변환 ^A X*_B = (^B X_A)^T, 렌치 (m, f) 순서"""
    pR = matmul(hat(p), R)
    return [R[i] + pR[i] for i in range(3)] + \
           [[0.0] * 3 + R[i] for i in range(3)]

def sp_inertia(m, c, Ic):
    """질량 m, 질량 중심 c, 중심 관성 Ic -> 6x6 (원점 기준)"""
    cx = hat(c)
    cc = matmul(cx, cx)
    top = [[Ic[i][j] - m * cc[i][j] for j in range(3)]
           + sc(m, cx[i]) for i in range(3)]
    bot = [sc(-m, cx[i]) + sc(m, I3[i]) for i in range(3)]
    return top + bot

def ad(V):
    """ad_V: 트위스트의 리 괄호 (MR 식 8.38)"""
    w, v = hat(V[:3]), hat(V[3:])
    return [w[i] + [0.0] * 3 for i in range(3)] + \
           [v[i] + w[i] for i in range(3)]

def rel_pose(tree, q):
    """부모 프레임에서 본 각 물체의 자세 (R, p)"""
    rel = [None]
    for i in range(1, len(tree)):
        par, R0, p0, ax = tree[i][:4]
        E = exp_so3(sc(q[i - 1], ax))
        rel.append((matmul(R0, E), p0))
    return rel

def to_child(R, p):
    """부모 -> 자식 운동 변환 ^i X_p"""
    Rt = transpose(R)
    return Xm(Rt, sc(-1.0, mv(Rt, p)))

def crba(tree, q):
    """부유기저 질량행렬 (6 + n): 합성 강체 관성에서 조립"""
    N = len(tree)
    rel = rel_pose(tree, q)
    Ic = [t[4] for t in tree]
    for i in range(N - 1, 0, -1):
        Xi, par = to_child(*rel[i]), tree[i][0]
        T = matmul(transpose(Xi), matmul(Ic[i], Xi))
        Ic[par] = [add(a, b) for a, b in zip(Ic[par], T)]
    M = [[0.0] * (5 + N) for _ in range(5 + N)]
    for r in range(6):
        M[r][:6] = Ic[0][r]
    for j in range(1, N):
        F, k = mv(Ic[j], list(tree[j][3]) + [0.0] * 3), j
        while k > 0:
            M[5 + k][5 + j] = M[5 + j][5 + k] = \
                sum(a * b for a, b in zip(tree[k][3], F[:3]))
            F, k = mv(Xf(*rel[k]), F), tree[k][0]
        for r in range(6):
            M[r][5 + j] = M[5 + j][r] = F[r]
    return M

def bias(tree, q, R0, V0, qd, g=(0.0, 0.0, -9.81)):
    """h(q, v): 속도 항과 중력(a=0), 첫 6행 = 몸통 렌치"""
    N = len(tree)
    rel = rel_pose(tree, q)
    gb = mv(transpose(R0), list(g))
    V, A = [V0], [[0.0] * 3 + sc(-1.0, gb)]
    for i in range(1, N):
        Xi, par = to_child(*rel[i]), tree[i][0]
        S = sc(qd[i - 1], tree[i][3]) + [0.0] * 3
        V.append(add(mv(Xi, V[par]), S))
        A.append(add(mv(Xi, A[par]), mv(ad(V[i]), S)))
    F = []
    for i in range(N):
        Iv = mv(tree[i][4], V[i])
        F.append(add(mv(tree[i][4], A[i]),
                     sc(-1.0, mv(transpose(ad(V[i])), Iv))))
    for i in range(N - 1, 0, -1):
        par = tree[i][0]
        F[par] = add(F[par], mv(Xf(*rel[i]), F[i]))
    tj = [sum(a * b for a, b in zip(tree[i][3], F[i][:3]))
          for i in range(1, N)]
    return F[0] + tj

def com_ag(tree, q, R0, p0):
    """질량 중심(세계), 총질량, 행렬 A_G (6 x nv)"""
    rel = rel_pose(tree, q)
    ab = [(I3, [0.0] * 3)]
    for i in range(1, len(tree)):
        Rp, pp = ab[tree[i][0]]
        ab.append((matmul(Rp, rel[i][0]),
                   add(pp, mv(Rp, rel[i][1]))))
    m = sum(t[5] for t in tree)
    cb = [0.0] * 3
    for (Ri, pi_), t in zip(ab, tree):
        cb = add(cb, sc(t[5] / m, add(pi_, mv(Ri, t[6]))))
    c = add(p0, mv(R0, cb))
    XG = Xf(R0, add(p0, sc(-1.0, c)))
    return c, m, matmul(XG, crba(tree, q)[:6])

def orth(R):
    """열 그람-슈미트 재직교화"""
    out = []
    for c in transpose(R):
        for u in out:
            d = sum(a * b for a, b in zip(c, u))
            c = [a - d * b for a, b in zip(c, u)]
        out.append(sc(1.0 / sqrt(sum(a * a for a in c)), c))
    return transpose(out)

def deriv(tree, x, tau):
    """비행 x=(p,R,q,V,qd,W): 몸통 구동 없음, W'=tau.qd"""
    n = len(tree) - 1
    R = [x[3 + 3 * i:6 + 3 * i] for i in range(3)]
    q, V = x[12:12 + n], x[12 + n:18 + n]
    qd = x[18 + n:18 + 2 * n]
    h = bias(tree, q, R, V, qd)
    rhs = [-v for v in h[:6]]
    rhs += [t - v for t, v in zip(tau, h[6:])]
    a = solve_lu(crba(tree, q), rhs)[0]
    dR = matmul(R, hat(V[:3]))
    return (mv(R, V[3:]) + [e for r in dR for e in r] + qd + a
            + [sum(t * s for t, s in zip(tau, qd))])

def rk4(f, x, t, dt):
    ax = lambda k, h: [a + h * b for a, b in zip(x, k)]
    k1 = f(t, x)
    k2 = f(t + dt / 2, ax(k1, dt / 2))
    k3 = f(t + dt / 2, ax(k2, dt / 2))
    k4 = f(t + dt, ax(k3, dt))
    y = [a + dt / 6 * (b + 2 * (c + d) + e)
         for a, b, c, d, e in zip(x, k1, k2, k3, k4)]
    R = orth([y[3 + 3 * i:6 + 3 * i] for i in range(3)])
    return y[:3] + [e for r in R for e in r] + y[12:]

① 구조와 중심 동역학 항등식.

질량행렬이 대칭이고 몸통 선형 블록이 $mI$이며, 운동 에너지와 중심 운동량이 링크별 직접 합과 같다(상대 $3.5\times10^{-16}$, 절대 $2.8\times10^{-14}$). $A_G$의 선형 행이 $m\,J_{\mathrm{com}}$과 $4.6\times10^{-9}$ 안에서 같다(차분 한계). $A_G$의 특이값 6개는 상태 60개에서 $0.694\sim46.64$이고 조건수가 최대 $67.2$이므로 계수 6이다. 이 값이 $A_G$ 행들이 독립이라는 보증은 아니며 시험한 상태에서의 관측이다. $\dot h_G$의 5점 차분이 $^{G}X^{*}_{0}F_0$와 중력 렌치의 합과 $7.7\times10^{-9}$ 안에서 같고, 질량 중심 가속도가 $f/m$과 $1.0\times10^{-10}$ 안에서 같다.

② 비행 구간의 운동량·에너지 보존.

관절 토크가 무작위인 비행에서 각운동량 $k$는 상수여야 하고 선운동량은 $l(t)=l(0)+mgt$여야 하며 에너지는 $E(t)=E(0)+\int\tau\cdot\dot q_j\,dt$여야 한다. RK4의 오차가 $\Delta t=0.01\to0.005\to0.0025$에서 $4.68\times10^{-5}\to3.07\times10^{-6}\to1.96\times10^{-7}$($k$)로 약 $1/15$씩 줄어 4차 수렴과 일치한다. 이 비행에서 몸통은 1 s 동안 $102.0^\circ$ 회전하고 관절 토크의 일은 $51.3$ J이며 초기 중심 운동량은 $k(0)=(-4.74,\ 4.22,\ -2.07)$ N·m·s, $l(0)=(5.99,\ -24.63,\ -62.79)$ N·s이다. 몸통이 크게 회전하는 동안에도 $k$가 변하지 않으므로 몸통 회전이 만드는 각운동량은 팔다리의 상대 운동이 만드는 각운동량과 상쇄된다.

③ 운동량 0 순환 운동의 순 회전(홀로노미).

초기 $h_G=0$에서 왼팔 어깨 두 관절을 원 경로로 1회 순환시키면 몸통이 처음 자세로 돌아오지 않는다. 순 회전은 수직축($z$) 성분이 지배적이며($\varepsilon=0.4$ rad에서 $9.106^\circ$, 수평 성분 $0.645^\circ$·$-0.432^\circ$) 총 회전각을 $\varepsilon^2$으로 나눈 값이 $\varepsilon=0.1$에서 $58.95$, $0.2$에서 $58.60$, $0.8$에서 $50.61$°/rad²이다. 작은 순환에서 회전이 $\varepsilon^2$에 비례하는 것은 순 회전이 경로가 둘러싼 넓이에 비례하는 구조와 일치한다(이 장은 그 해석을 이론적으로 유도하지 않았다). 운동량 구속 적분(첫 경로)과 몸통 여섯 행 전체 동역학의 적분(둘째 경로)이 $4.3\times10^{-11}\sim2.3\times10^{-9}$ rad 안에서 같다. 두 경로는 같은 RK4 간격을 사용하며 한 경로는 일차 속도 구속식, 다른 경로는 이차 동역학 방정식이다.

④ ZMP와 중심 동역학.

세 경우에서 접촉 렌치로 구한 ZMP와 $(\ddot c,\dot k)$ 식이 $10^{-5}$ m 이내로 같다(임의 80개에서 $4.2\times10^{-11}$ m). 경우 A는 $\ddot c_z=0$·$\dot k=0$이어서 식이 LIPM과 같다. 경우 B는 몸통 피치 각가속도 $5$ rad/s²가 $\dot k_y=20.69$ N·m를 만들고 ZMP가 LIPM 식의 $0.0413$ m가 아니라 $-0.0044$ m가 되어 $4.6$ cm 어긋난다($\dot k_y/(mg)=4.6$ cm). 경우 C는 수직 가속 $2$ m/s²로 분모가 $\ddot c_z+g$가 되어 ZMP가 $-0.0858$ m에서 $-0.0710$ m로 이동한다. 각운동량 변화와 수직 가속을 무시하는 LIPM은 이 두 효과가 클수록 ZMP를 틀리게 예측한다.

⑤ 라이브러리 대조(실행 2026-11-05). Pinocchio 4.1.0의 crba·nonLinearEffects·rnea·centerOfMass·ccrba·computeCentroidalMomentumTimeVariation·aba를 같은 상태·가속도에서, SciPy 1.18.1의 solve_ivp(DOP853)를 비행 적분(Pinocchio aba로 가속도)과 홀로노미(Pinocchio ccrba의 $A_G$로 속도 구속)에서, MuJoCo 3.17.0의 mj_fullM·mj_forward·mj_subtreeVel과 RK4 적분기를 관절 질량행렬·비행 가속도·질량 중심·운동량·1 s 비행에서 비교했다. 좌표 대응은 Pinocchio의 $(v,\omega)$ 대 이 장의 $(\omega,v)$ 치환, Pinocchio 쿼터니언 $(x,y,z,w)$, MuJoCo 쿼터니언 $(w,x,y,z)$·병진 속도의 세계 좌표 변환이다. 스크립트는 robotics/fig-src/3.13-libcheck.py, 출력은 robotics/fig-src/data/3.13-libcheck-output.txt에 있다.

질량행렬·바이어스 힘·역동역학·질량 중심·$A_G$·$h_G$가 Pinocchio와 $1.7\times10^{-13}$ 이내이고, $\dot h_G$는 Pinocchio의 값과 $5.1\times10^{-8}$(차분 한계) 및 $2.1\times10^{-13}$(이 장의 식)으로 같다. 비행 적분이 SciPy와 Pinocchio aba로 만든 기준해와 몸통 회전에서 $1.7\times10^{-8}$ rad, 관절 속도에서 $1.5\times10^{-6}$ 안에서 같으며 이는 이 장의 $\Delta t=2.5$ ms RK4의 오차(풀이 예제 ②)와 같은 크기이다. 순환 운동의 순 회전은 Pinocchio ccrba로 적분해도 $9.1394^\circ$로 같다($2.6\times10^{-13}$ rad). MuJoCo는 관절 질량행렬·비행 가속도·질량 중심·운동량에서 $7.4\times10^{-13}$ 이하로 같고, 1 ms RK4 비행 1 s의 최종 상태가 이 장과 몸통 회전 $4.2\times10^{-6}$ rad·관절각 $7.4\times10^{-7}$ rad 안에서 같으며 이 간격에서 MuJoCo 각운동량의 변화가 $6.8\times10^{-6}$ N·m·s이다. 이상 징후는 발견하지 못했다.

한계. 접촉이 없는 비행과 접촉 렌치를 임의로 준 항등식 시험이며 실제 접촉(발-지면, 3.12)과 결합한 보행 시뮬레이션은 다루지 않았다. 회전 관절만 있는 트리이고 병진·구면·폐연쇄(3.11)·유연 관절(3.9)은 포함하지 않았다. 홀로노미를 한 관절 쌍의 원 경로로만 시험했고 최적 경로나 최소 노력 경로는 구하지 않았다. 중심 동역학을 사용한 계획·제어(MPC, 14.17)와 실제 로봇의 모델 오차 영향은 시험하지 않았다.

견해: 보행·점프의 계획과 상위 제어에는 중심 동역학(여섯 식)을 축약 모형으로 두고 전신 제어(12.7)가 접촉력과 관절 토크를 전체 동역학으로 구현하는 계층 구성을 기본으로 둔다고 본다. 근거는 이 장에서 중심 동역학 항등식이 $n_v=18$의 전체 동역학 첫 여섯 행에서 $10^{-8}$ 이내로 나오고 관절 토크가 그 식에 나타나지 않아 접촉력과 질량 중심·각운동량만으로 계획 문제가 닫힌다는 점, 그리고 ZMP·LIPM이 이 식의 특수 경우이며(경우 A에서 일치) 각운동량 변화와 수직 가속을 버린 근사라는 점이다. 관절 한계·자기 충돌·팔다리의 질량 분포가 중요한 과제(예: 큰 각운동량 동작)에서는 축약 모형이 부족할 수 있으며 이 장은 이 한계를 계산하지 않았다.

3.13.6 설계 트레이드오프 — 부유기저 로봇의 동역학 모형 선택

모형은 다음 중에서 고른다.

• ① 전체 다물체 동역학. 질량행렬 $M(q)$·바이어스 힘 전체를 사용한다. 정확하나 자유도가 크고 접촉 순서와 결합하면 계획이 비싸다.
• ② 중심 동역학 + 완전한 기구학. $\dot h_G$ 여섯 식과 관절 기구학을 함께 둔다. 관절 토크를 제거해 계획 변수가 줄고 접촉력과 질량 중심·각운동량으로 닫힌다. 팔다리의 관성 효과는 $A_G$에 담기지만 관절 토크 한계는 별도 제약이다.
• ③ 단일 강체·LIPM. 질량 중심과 지지 다각형만 사용한다. 가장 단순하며 $\dot k=0$·$\ddot c_z=0$을 가정한다.

견해: 휴머노이드 보행의 실시간 계획에는 ②를, 정상 보행의 단순한 안정성 해석에는 ③을, 시뮬레이션 검증과 전신 토크 계산에는 ①을 둔다고 본다. 근거는 이 장에서 ③이 경우 B에서 $4.6$ cm의 ZMP 오차를 내고(각운동량 변화) ②는 이 항을 포함하며, ①은 항등식의 기준이라는 점이다. 보행 속도와 팔 운동이 커질수록 ③의 오차가 커지며 이 장은 실제 보행 궤적의 오차를 계산하지 않았다.

3.13.7 응용 축 비교

이 표는 이 책의 정리이며 응용별 수치 요구는 이 장이 확인하지 않았다.

3.13.8 관련 규격·프로토콜

부유기저를 다루는 기계 안전·인증 규격은 확인하지 못했다. 라이브러리 이식에서는 좌표 순서(선형 대 각 먼저), 쿼터니언 순서, 속도 좌표계(몸체 대 세계)를 확인해야 한다.

3.13.9 관련 규제·인증 요구와의 연결

부유기저 동역학 자체를 규정하는 법령·인증 요건은 확인하지 못했다. 휴머노이드의 낙상과 보호 정지 거동, 충돌 안전의 평가는 총 운동량과 에너지에서 출발하므로 이 장의 중심 운동량·운동 에너지 계산이 입력이 된다. 낙상 관리는 14.24, 휴머노이드 안전 아키텍처는 33.7, 인간-로봇 충돌 안전은 26.9과 이어진다. 이 장은 규정의 조항을 확인하지 않았다 (확인 필요).

3.13.10 한계·유의점·미해결

• 접촉 결합. 이 장은 접촉력을 입력(항등식 시험)으로만 두었다. 접촉 모형(3.12)과 결합한 부유기저 보행 시뮬레이션과 접촉 순서의 영향은 다루지 않았다.
• 모형 범위. 회전 관절만 있는 트리이고 몸통 하나, 모든 관성이 대각이다. 병진·구면 관절, 폐연쇄, 유연 관절, 관절 마찰·전동기 관성(3.8·3.9)은 포함하지 않았다.
• 홀로노미. 한 관절 쌍의 원 경로만 계산했다. 순 회전이 경로 넓이에 비례한다는 해석, 최소 에너지 경로, 가제어성은 다루지 않았다.
• 수치. 5점 차분은 약 $10^{-8}$까지만 검증하며 RK4는 간격 $\Delta t$에서 4차이다. 자세 적분은 행렬 재직교화로 처리했고 쿼터니언 적분과 비교하지 않았다.
• 원문 미확인. CCRBA·$O(N)$ 계산 알고리즘, 중심 동역학 기반 계획 문헌, 고양이 낙하 문헌의 본문은 읽지 못했다.
• 좌표 규약. 라이브러리마다 선형 대 각 순서, 쿼터니언 순서, 몸체 대 세계 속도가 다르다. 이 장의 대조는 모형 하나에서의 대응이다.

3.13.11 체크포인트·연구 질문·출처

체크포인트:

1. $\partial T/\partial V_0=h_0$를 보이고 $A_G={}^{G}X^{*}_{0}M[0\!:\!6,:]$를 유도하라.
2. $M_{bb}$의 선형 블록이 $mI$이고 $A_G$의 선형 행이 $m\,J_{\mathrm{com}}$임을 보이라.
3. 중심 동역학 $\dot h_G$에 중력의 모멘트가 나타나지 않는 이유와 관절 토크가 나타나지 않는 이유를 설명하라.
4. $h_G=0$에서 $V_0=-A_b^{-1}A_j\dot q_j$를 유도하고 이 구속이 적분되지 않을 수 있는 이유를 설명하라.
5. 접촉 렌치와 $(\ddot c,\dot k)$에서 ZMP 식을 유도하고 LIPM이 되는 조건을 적으라.
6. 쿼터니언 설정에서 $n_q\ne n_v$인 이유와 지수 사상 갱신식을 적으라.

연구 질문:

• 닫힌 관절 경로의 순 회전을 최대로 하는(또는 에너지를 최소로 하는) 경로를 이 장의 $A_G$로 어떻게 구하는가(이 장은 시험하지 않았다)?
• 중심 동역학 축약 모형과 전체 동역학의 불일치(토크 한계·관절 동역학)를 보행 시뮬레이션에서 어떻게 정량화하는가?
• 접촉 순서가 바뀌는 구간에서 $\dot h_G$의 불연속(충격, 3.12)을 중심 동역학에 어떻게 넣는가?

1차 출처(조회 2026-11-05).

• K. M. Lynch, F. C. Park, Modern Robotics, Cambridge University Press, 2017(사전 인쇄본 2017-05-03) — 8.2.2절을 직접 읽었다.
• Pinocchio 4.1.0(ccrba·computeCentroidalMomentum·computeCentroidalMomentumTimeVariation·computeCentroidalMap·centerOfMass·crba·rnea·aba·integrate의 함수 설명)과 J. Carpentier 외, "The Pinocchio C++ library," IEEE/SICE SII 2019, 614–619, doi:10.1109/sii.2019.8700380 — 서지만 확인. MuJoCo 3.17.0 문서(자유 관절, mj_subtreeVel) — 직접 읽었다.
• D. E. Orin, A. Goswami, "Centroidal momentum matrix of a humanoid robot: Structure and properties," IROS 2008, 653–659, doi:10.1109/iros.2008.4650772 — 서지만 확인. D. E. Orin, A. Goswami, S.-H. Lee, "Centroidal dynamics of a humanoid robot," Autonomous Robots 35(2–3), 161–176, 2013, doi:10.1007/s10514-013-9341-4 — 서지만 확인. P. M. Wensing, D. E. Orin, "Improved computation of the humanoid centroidal dynamics and application for whole-body control," Int. J. Humanoid Robotics 13(1), 1550039, 2016, doi:10.1142/s0219843615500395 — 서지만 확인. S.-H. Lee, A. Goswami, "A momentum-based balance controller for humanoid robots on non-level and non-stationary ground," Autonomous Robots 33(4), 399–414, 2012, doi:10.1007/s10514-012-9294-z — 서지만 확인.
• H. Dai, A. Valenzuela, R. Tedrake, "Whole-body motion planning with centroidal dynamics and full kinematics," Humanoids 2014, 295–302, doi:10.1109/humanoids.2014.7041375 — 서지만 확인. A. Herzog, S. Schaal, L. Righetti, "Structured contact force optimization for kino-dynamic motion generation," IROS 2016, 2703–2710, doi:10.1109/iros.2016.7759420 — 서지만 확인. B. Ponton 외, "A convex model of humanoid momentum dynamics for multi-contact motion generation," Humanoids 2016, 842–849, doi:10.1109/humanoids.2016.7803371 — 서지만 확인.
• S. Kajita 외, "The 3D linear inverted pendulum mode," IROS 2001, 239–246, doi:10.1109/iros.2001.973365 — 서지만 확인. M. Vukobratović, B. Borovac, "Zero-moment point — thirty five years of its life," Int. J. Humanoid Robotics 1(1), 157–173, 2004, doi:10.1142/s0219843604000083 — 서지만 확인. M. B. Popović, A. Goswami, H. Herr, "Ground reference points in legged locomotion," IJRR 24(12), 1013–1032, 2005, doi:10.1177/0278364905058363 — 서지만 확인.
• T. R. Kane, M. P. Scher, "A dynamical explanation of the falling cat phenomenon," Int. J. Solids Struct. 5(7), 663–670, 1969, doi:10.1016/0020-7683(69)90086-9 — 서지만 확인. R. Montgomery, "Gauge theory of the falling cat," in Dynamics and Control of Mechanical Systems, 193–218, 1993, doi:10.1090/fic/001/09 — 서지만 확인. R. Featherstone, Rigid Body Dynamics Algorithms, Springer, 2008, doi:10.1007/978-1-4899-7560-7 — 서지만 확인.
• numpy 2.5.4, SciPy 1.18.1(integrate.solve_ivp), MuJoCo 3.17.0의 계산 대조.

다음 장. 4.1은 로봇 기계 설계 개관(요구조건에서 사양·설계 루프까지)을 다룬다. 이 장까지의 동역학(관성·질량행렬·접촉·중심 운동량)이 관절 토크와 질량 분포의 요구조건이 되어 제4부 기계 설계로 이어진다.