제3부 동역학

3.6 동역학 라이브러리와 동역학 도함수: Pinocchio·RBDL·Drake MultibodyPlant·MuJoCo — 해석적 도함수와 자동 미분

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

이 장은 동역학 라이브러리 네 가지(Pinocchio·RBDL·Drake MultibodyPlant·MuJoCo)가 동역학과 그 도함수를 어떻게 제공하는지 비교하고, 동역학 도함수 $\partial\tau/\partial(q,\dot q,\ddot q)$·$\partial\ddot q/\partial(q,\dot q,\tau)$를 계산하는 다섯 방식, 즉 유한 차분·복소수 스텝·순방향 자동 미분·그래프 기반 자동 미분·해석적 도함수를 구현해 정확도와 비용을 계산한다. 도함수는 선형화·미분 동적 계획법(DDP)·직접 콜로케이션·식별의 감도에 필요한 입력이다. 이 장은 3.5의 RNEA·ABA를 사용하고, 해석적 도함수를 공간 벡터 재귀의 접선 전파로 직접 유도해 복소수 스텝·자동 미분·네 라이브러리·CasADi와 대조한다. RNEA·ABA는 3.5, 질량행렬의 성질은 3.4, 부유 기저의 도함수는 3.13, 미분 가능 시뮬레이션은 16.10, 수치 해석과 자동 미분의 일반론은 1.18에서 다룬다.

배우는 것:

• 도함수의 구조 — $\partial\tau/\partial\ddot q=M$, 순동역학 도함수 $=-M^{-1}\,\partial\tau/\partial(q,\dot q)$, 전이 행렬 $A$·$B$.
• 다섯 계산 방식 — 유한 차분의 오차 곡선, 복소수 스텝, 이중수 자동 미분, CasADi 그래프 미분, 해석적 접선 전파.
• 라이브러리 지원 — Pinocchio의 해석적 도함수, Drake의 AutoDiffXd, MuJoCo의 유한 차분 mjd_*와 전치 관례, RBDL의 도함수 부재.

선수 지식: 1.18(수치 미분·자동 미분), 3.2(RNEA), 3.4(운동방정식), 3.5(CRBA·ABA·공간 벡터 대수).

특히 중요한 독자: DDP·iLQR·MPC의 동역학 도함수를 구현하는 최적 제어 엔지니어, 동역학 라이브러리를 선택해 모델 기반 제어에 통합하는 개발자, 자동 미분과 해석적 도함수의 정확도·비용을 비교하려는 연구자.

출처 방침. 기준 시점은 2026-11-05이다. 직접 읽은 자료: MuJoCo 문서의 계산 개관(동역학 도함수를 위한 함수 두 개 mjd_transitionFD·mjd_inverseFD가 유한 차분을 사용한다는 서술)과 두 함수의 파이썬 독스트링(mjd_inverseFD의 출력이 제어 이론 관례의 전치라는 서술). Pinocchio 4.1.0의 computeRNEADerivatives·computeABADerivatives·computeMinverse 독스트링. RBDL 2.6.0 소스 include·src 전체에서 derivative를 검색한 결과(동역학 도함수 함수가 없다). Drake·CasADi는 실행으로 확인했다. 서지만 확인한 것(Crossref로 실재를 확인했고 본문은 읽지 못했다): Carpentier·Mansard(2018, 강체 동역학 알고리즘의 해석적 도함수), Carpentier 외(2019, Pinocchio), Squire·Trapp(1998)·Martins·Sturdza·Alonso(2003)(복소수 스텝), Griewank·Walther(2008, 자동 미분), Andersson 외(CasADi, 2019), Mastalli 외(2020, Crocoddyl). 이 서지들의 알고리즘·연산량은 읽지 못했으므로 서술하지 않는다 (확인 필요). 이 장의 해석적 도함수 식은 문헌이 아니라 이 장이 공간 벡터 대수에서 직접 유도하고 복소수 스텝·자동 미분·라이브러리와 대조해 확인했다. 팔·트리·사슬은 3.4·3.5의 가정이다.

3.6.2 개념 — 같은 야코비안을 다섯 방식으로 계산한다

결론부터 적으면 다음과 같다. 역동역학 $\tau=\mathrm{RNEA}(q,\dot q,\ddot q)$의 $\ddot q$에 대한 도함수는 질량행렬 $M(q)$이고 $q$·$\dot q$에 대한 도함수가 $\partial\tau/\partial q$·$\partial\tau/\partial\dot q$이다. 순동역학 $\ddot q=\mathrm{ABA}(q,\dot q,\tau)$의 도함수는 이 값에서 $-M^{-1}\,\partial\tau/\partial(q,\dot q)$와 $M^{-1}$로 얻는다. 같은 야코비안을 유한 차분, 복소수 스텝, 자동 미분, 해석적 도함수로 계산할 수 있고 정확도가 크게 다르다. 유한 차분은 간격 $h$를 선택해야 하며 최선의 간격에서도 오차가 전진 차분 $10^{-8}$·중심 차분 $10^{-11}$ 수준이다. 복소수 스텝·자동 미분·해석적 도함수는 반올림 수준($10^{-15}$)에서 같다. 해석적 도함수는 알고리즘의 구조를 이용하므로 비용이 가장 낮을 수 있으나 유도와 구현이 필요하다. 라이브러리는 지원 범위가 다르다. Pinocchio는 RNEA·ABA의 해석적 도함수를, Drake는 스칼라 형식을 AutoDiffXd로 바꾸어 자동 미분을, MuJoCo는 유한 차분 함수를 제공하고 RBDL 2.6.0은 동역학 도함수 함수를 제공하지 않는다.

이 장의 계산 결과를 미리 적으면 다음과 같다. ① 6R 팔에서 해석적 도함수와 이중수 자동 미분이 복소수 스텝과 상대 $1.5\times10^{-14}$ 안에서 같고(가장 나쁜 블록은 $\partial\ddot q/\partial\dot q$) 중심 차분($h=10^{-6}$)은 $10^{-9}\sim10^{-6}$, 전진 차분($h=10^{-8}$)은 $10^{-7}\sim10^{-4}$이다. ② 차분 간격에 대한 오차는 전진 차분이 $h=10^{-8}$에서 $2.0\times10^{-8}$, 중심 차분이 $10^{-5}$에서 $1.2\times10^{-11}$로 최소이고, 복소수 스텝은 $h\le10^{-8}$에서 $5\times10^{-16}$ 이하로 일정하다. ③ 해석적 도함수의 곱셈 횟수는 RNEA의 $9.8$배($n=6$)~$64.6$배($n=48$)이며 중심 차분($4n$회 RNEA)의 $0.41\sim0.34$이다. ④ 해석적 도함수로 만든 세미 암시적 오일러 전이 행렬 $A$·$B$가 복소수 스텝과 $4.4\times10^{-15}$ 안에서 같다. ⑤ 라이브러리 네 가지가 같은 도함수를 재현한다: Pinocchio $1.6\times10^{-13}$, CasADi $1.7\times10^{-14}$, Drake $1.1\times10^{-15}$ 이하, MuJoCo(유한 차분) $8.2\times10^{-6}$. 그리고 Pinocchio의 해석적 도함수는 파이썬 중심 차분 루프보다 $n=96$에서 RNEA $66$배·ABA $15$배 빠르다.

3.6.3 원리

3.6.3.1 동역학 도함수의 구조

역동역학 $\tau=\mathrm{RNEA}(q,\dot q,\ddot q)=M(q)\ddot q+h(q,\dot q)$($h=C\dot q+g$)를 세 변수로 미분하면 다음이다.

$$\frac{\partial\tau}{\partial\ddot q}=M(q),\qquad \frac{\partial\tau}{\partial q}=\frac{\partial M}{\partial q}\ddot q+\frac{\partial h}{\partial q},\qquad \frac{\partial\tau}{\partial\dot q}=\frac{\partial h}{\partial\dot q}.$$

순동역학 $\ddot q=\mathrm{ABA}(q,\dot q,\tau)$는 $\tau=\mathrm{RNEA}(q,\dot q,\ddot q)$의 역함수이므로 전미분 $d\tau=\tfrac{\partial\tau}{\partial q}dq+\tfrac{\partial\tau}{\partial\dot q}d\dot q+M\,d\ddot q$에서 $d\ddot q$를 풀면 다음이다(모든 편도함수는 $\ddot q=\mathrm{ABA}(q,\dot q,\tau)$에서 평가한다).

$$\frac{\partial\ddot q}{\partial q}=-M^{-1}\frac{\partial\tau}{\partial q},\qquad \frac{\partial\ddot q}{\partial\dot q}=-M^{-1}\frac{\partial\tau}{\partial\dot q},\qquad \frac{\partial\ddot q}{\partial\tau}=M^{-1}.$$

그러므로 순동역학 도함수는 RNEA 도함수와 $M^{-1}$(3.5)만 있으면 된다. 회전 관절만 있는 이 장은 $q$의 도함수를 일반 좌표 도함수로 다룬다. 쿼터니언·부유 기저는 도함수가 접선 공간에서 정의되며 3.13에서 다룬다. MuJoCo의 mjd_inverseFD는 이 장의 $\partial\tau/\partial q$를 제어 이론 관례의 전치로 내며(독스트링에 적혀 있다) 이 장의 대조에서 확인했다.

3.6.3.2 유한 차분과 복소수 스텝

전진 차분 $\bigl(f(x+h)-f(x)\bigr)/h$의 오차는 절단 $\tfrac h2\lvert f''\rvert$와 반올림 $\varepsilon\lvert f\rvert/h$의 합이어서 $h\approx\sqrt\varepsilon\approx10^{-8}$에서 최소 $\sim10^{-8}$이다. 중심 차분의 오차는 $\tfrac{h^2}{6}\lvert f'''\rvert+\varepsilon\lvert f\rvert/h$이어서 $h\approx\varepsilon^{1/3}\approx10^{-5}$에서 최소 $\sim10^{-11}$이다. 복소수 스텝은 $f$가 해석적일 때 $f'(x)\approx\operatorname{Im}f(x+ih)/h$이고 뺄셈이 없어 반올림 오차가 없으며 절단 오차 $\tfrac{h^2}{6}f'''$만 남으므로 $h$를 $10^{-30}$처럼 작게 둘 수 있다(Squire·Trapp, Martins 외는 서지만 확인). 조건은 함수가 복소수 입력을 받는 해석적 식으로만 이루어져 있어야 하며(절댓값·최솟값·켤레 금지) RNEA·ABA는 이를 만족한다. 비용은 입력당 복소수 평가 1회이다.

3.6.3.3 자동 미분

순방향 모드는 각 수에 접선을 붙인 이중수 $x+\dot x\varepsilon$($\varepsilon^2=0$)로 알고리즘을 실행해 한 입력 방향의 도함수를 한 번의 실행으로 얻는다. $(q,\dot q)$ 전체는 $2n$회 실행이다. 역방향 모드는 계산 그래프를 거꾸로 훑어 한 출력의 모든 입력 도함수를 한 번에 얻으며 출력이 $n$개인 $\tau$는 $n$회이다(일반론은 1.18, Griewank·Walther(2008)는 서지만 확인). 자동 미분은 코드를 미분하므로 절단 오차가 없고 반올림 수준에서 정확하다. Drake는 MultibodyPlant의 스칼라 형식을 AutoDiffXd로 바꾼 ToAutoDiffXd로, CasADi는 기호 변수(SX)로 알고리즘을 추적해 jacobian으로 그래프를 미분한다. 이 장은 이중수(순수 파이썬)·CasADi·Drake를 모두 시험한다.

3.6.3.4 해석적 도함수 — 공간 벡터 재귀의 접선 전파

3.5의 RNEA는 $v_i=X_iv_\lambda+S_i\dot q_i$, $a_i=X_ia_\lambda+S_i\ddot q_i+v_i\times S_i\dot q_i$, $f_i=I_ia_i+v_i\times^{*}I_iv_i$, $F_i=f_i+\sum_jX_j^TF_j$, $\tau_i=S_i^TF_i$이다. 관절 $i$의 회전은 좌표 변환을 $\partial X_i/\partial q_i=-(S_i\times)X_i$로 바꾼다(링크 프레임이 회전하므로 좌표가 반대로 회전한다). 입력 $q_k$ 방향의 접선 $\delta v_i,\delta a_i,\delta F_i$는 $k$의 부분 트리에서만 0이 아니며 다음 식으로 전방 전파된다($\delta_{ik}$는 $i=k$일 때만 1).

$$\delta v_i=X_i\delta v_\lambda-\delta_{ik}\,S_i\times(X_iv_\lambda),\qquad \delta a_i=X_i\delta a_\lambda-\delta_{ik}\,S_i\times(X_ia_\lambda)+\delta v_i\times S_i\dot q_i,$$ $$\delta f_i=I_i\delta a_i+\delta v_i\times^{*}I_iv_i+v_i\times^{*}I_i\delta v_i .$$

후방 순환에서 $\delta F_\lambda\mathrel{+}=X_i^T\bigl(\delta F_i+\delta_{ik}\,S_i\times^{*}F_i\bigr)$이고 $\partial\tau_i/\partial q_k=S_i^T\delta F_i$이다($\delta_{ik}$ 항은 $\partial X_i^T/\partial q_i$에서 나온다). $\dot q_k$ 방향은 $\delta v_k=S_k$, $\delta a_k\mathrel{+}=v_k\times S_k$로 시작하고 $X$의 미분 항이 없다. 중력은 기저 가속도이므로 도함수에 나타나지 않는다. $\partial\tau/\partial\ddot q$는 CRBA의 $M$이다. 이 장의 구현은 입력마다 부분 트리를 전파해 연산량이 $O(n\cdot\text{부분 트리 크기})$이며 사슬에서 $O(n^{2})$이다. Carpentier·Mansard(2018)는 이 도함수의 효율적 알고리즘을 다루는 것으로 서지만 확인했다 (확인 필요).

3.6.3.5 전이 행렬과 최적 제어

세미 암시적 오일러 $v^{+}=v+h\,\ddot q$, $q^{+}=q+h\,v^{+}$($\ddot q=\mathrm{ABA}(q,v,\tau)$)의 전이 행렬은 $a_q=\partial\ddot q/\partial q$, $a_v=\partial\ddot q/\partial\dot q$로 다음이다.

$$A=\frac{\partial(q^{+},v^{+})}{\partial(q,v)}=\begin{pmatrix}1+h^{2}a_q&h(1+h\,a_v)\\h\,a_q&1+h\,a_v\end{pmatrix},\qquad B=\frac{\partial(q^{+},v^{+})}{\partial\tau}=\begin{pmatrix}h^{2}M^{-1}\\h\,M^{-1}\end{pmatrix}.$$

연속 시간 선형화는 $A_c=\begin{pmatrix}0&1\\a_q&a_v\end{pmatrix}$, $B_c=\begin{pmatrix}0\\M^{-1}\end{pmatrix}$이다. DDP·iLQR(12.17), 직접 콜로케이션(11.8), 선형 MPC(12.16)가 이 행렬을 매 시간 간격마다 요구하므로 도함수의 비용이 알고리즘 전체의 비용을 정한다. Crocoddyl 등 최적 제어 라이브러리는 12.20에서 다룬다(Mastalli 외(2020)는 서지만 확인).

3.6.3.6 라이브러리의 도함수 지원

3.6.4 알고리즘

RNEA 도함수(tree, q, q', q''):  값 순환으로 v_i, a_i, F_i(부분 트리 합력), X_i v_λ, X_i a_λ 를 저장
    k = 1..n, wrt ∈ {q, q'}:   dv = da = dF = 0 (k의 부분 트리 밖은 건너뛴다)
        i가 k의 부분 트리에서 전방:  dv_i ← X_i dv_λ ;  da_i ← X_i da_λ
              wrt = q,  i = k :  dv_i −= S_i × (X_i v_λ) ;  da_i −= S_i × (X_i a_λ)
              wrt = q', i = k :  dv_i += S_i
              da_i += dv_i × (q'_i S_i) ;  (wrt = q', i = k :  da_i += v_i × S_i)
              dF_i ← I_i da_i + dv_i ×* (I_i v_i) + v_i ×* (I_i dv_i)
        i = n..1 후방:  D[i][k] ← S_i · dF_i ;  t ← dF_i (+ S_i ×* F_i  (wrt = q, i = k)) ;  dF_λ += X_i^T t
순동역학 도함수:  q'' ← ABA ;  (D_q, D_v) ← RNEA 도함수(q, q', q'') ;  M ← CRBA ;  ∂q''/∂q = −M^-1 D_q, ∂q''/∂q' = −M^-1 D_v, ∂q''/∂τ = M^-1
복소수 스텝:  ∂f/∂x_j = Im f(x + i h e_j) / h  (h = 10^-30)       순방향 AD:  x_j에 ε를 붙여 f 실행 → ε 성분

3.6.5 풀이 예제 — 다섯 방식의 정확도·비용, 전이 행렬, 라이브러리

결론부터 적으면 다음과 같다. ① 해석적 도함수·이중수·복소수 스텝이 반올림 수준에서 같고 차분은 $10^{-9}$ 이상 어긋난다. ② 차분 오차는 $h$에 대해 V자 곡선이고 복소수 스텝은 평평하다. ③ 이 장의 단순한 해석적 구현은 중심 차분의 약 $0.35\sim0.4$배 곱셈이 든다. ④ 전이 행렬이 같다. ⑤ 라이브러리 네 가지가 일치하고 Pinocchio의 해석적 도함수가 파이썬 차분 루프보다 RNEA는 $11\sim66$배, ABA는 $14\sim36$배 빠르다.

구현. Dual·tsin·tcos·rodrigues_g·jac_fd·jac_cs·jac_ad·rnea_deriv·aba_deriv·euler_step·transition. 3.5의 rnea·aba·crba를 불러오고 rodrigues를 복소수·이중수를 받는 rodrigues_g로 교체한다. 표·그림·표본 추출은 스크립트에 있다.

class Dual:
    """순방향 자동 미분의 이중수 v + d ε (ε² = 0)"""
    def __init__(s, v, d=0.0):
        s.v, s.d = v, d
    def __add__(s, o):
        o = o if isinstance(o, Dual) else Dual(o)
        return Dual(s.v + o.v, s.d + o.d)
    __radd__ = __add__
    def __neg__(s):
        return Dual(-s.v, -s.d)
    def __sub__(s, o):
        return s + (-o)
    def __rsub__(s, o):
        return (-s) + o
    def __mul__(s, o):
        o = o if isinstance(o, Dual) else Dual(o)
        return Dual(s.v * o.v, s.v * o.d + s.d * o.v)
    __rmul__ = __mul__
    def __truediv__(s, o):
        o = o if isinstance(o, Dual) else Dual(o)
        d = (s.d * o.v - s.v * o.d) / o.v ** 2
        return Dual(s.v / o.v, d)
    def __rtruediv__(s, o):
        return Dual(o) / s

def tsin(x):
    if isinstance(x, Dual):
        return Dual(math.sin(x.v), math.cos(x.v) * x.d)
    if isinstance(x, complex):
        return cmath.sin(x)
    return math.sin(x)
def tcos(x):
    if isinstance(x, Dual):
        return Dual(math.cos(x.v), -math.sin(x.v) * x.d)
    if isinstance(x, complex):
        return cmath.cos(x)
    return math.cos(x)

def rodrigues_g(a, q):
    """float·복소수·이중수를 받는 로드리게스 식"""
    K = rb66.skew(a)
    K2 = rb66.matmul(K, K)
    return [[(i == j) + tsin(q) * K[i][j]
             + (1 - tcos(q)) * K2[i][j] for j in range(3)]
            for i in range(3)]
m.rodrigues = rodrigues_g     # 3.5의 tf_all이 사용한다

def jac_fd(f, x, h, central=True):
    """f(x)의 야코비안 (열 j = 입력 j)"""
    f0 = None if central else f(x)
    cols = []
    for j in range(len(x)):
        xp = list(x)
        xp[j] += h
        if central:
            xm = list(x)
            xm[j] -= h
            fp, fm = f(xp), f(xm)
            cols.append([(a - b) / (2 * h)
                         for a, b in zip(fp, fm)])
        else:
            cols.append([(a - b) / h
                         for a, b in zip(f(xp), f0)])
    return [list(r) for r in zip(*cols)]

def jac_cs(f, x, h=1e-30):
    """복소수 스텝: ∂f/∂x_j = Im f(x + i h e_j) / h"""
    cols = []
    for j in range(len(x)):
        xc = [complex(v) for v in x]
        xc[j] += 1j * h
        cols.append([y.imag / h for y in f(xc)])
    return [list(r) for r in zip(*cols)]

def jac_ad(f, x):
    """순방향 자동 미분: 입력 j마다 이중수 1회"""
    cols = []
    for j in range(len(x)):
        xd = [Dual(v, float(i == j)) for i, v in enumerate(x)]
        cols.append([y.d for y in f(xd)])
    return [list(r) for r in zip(*cols)]

def rnea_deriv(tree, q, qd, qdd, gam=(0.0, 0.0, -9.81)):
    """해석적 ∂τ/∂q, ∂τ/∂q'. 입력 k마다 접선을 전방으로
    전파하고 후방에서 모은다. ∂X_i/∂q_i = −S_i× X_i."""
    mv, cr, cf = m.mv, m.crm, m.crf
    n, par = len(q), tree['par']
    X, S = m.tf_all(tree, q)
    G = [m.inertia6(*P) for P in tree['P']]
    a0 = [0.0] * 3 + [-g for g in gam]
    v, a, F = [0] * n, [0] * n, [0] * n
    xv, xa = [0] * n, [0] * n
    for i in range(n):                        # 값의 전방 순환
        vp = v[par[i]] if par[i] >= 0 else [0.0] * 6
        ap = a[par[i]] if par[i] >= 0 else a0
        xv[i], xa[i] = mv(X[i], vp), mv(X[i], ap)
        vJ = vsc(qd[i], S[i])
        v[i] = vadd(xv[i], vJ)
        a[i] = vadd(vadd(xa[i], vsc(qdd[i], S[i])),
                    mv(cr(v[i]), vJ))
        Gv = mv(G[i], v[i])
        F[i] = vadd(mv(G[i], a[i]), mv(cf(v[i]), Gv))
    for i in reversed(range(n)):              # 부분 트리 합력
        if par[i] >= 0:
            Xt = m.tr(X[i])
            F[par[i]] = vadd(F[par[i]], mv(Xt, F[i]))
    out = []
    for wrt in ('q', 'qd'):
        D = [[0.0] * n for _ in range(n)]
        for k in range(n):
            dv, da, dF = [None] * n, [None] * n, [None] * n
            for i in range(n):           # k의 부분 트리만
                p = par[i]
                if i != k and (p < 0 or dv[p] is None):
                    continue
                z = [0.0] * 6
                dvi = mv(X[i], dv[p]) if i != k else z
                dai = mv(X[i], da[p]) if i != k else z
                if i == k and wrt == 'q':
                    dvi = vsub(dvi, mv(cr(S[i]), xv[i]))
                    dai = vsub(dai, mv(cr(S[i]), xa[i]))
                if i == k and wrt == 'qd':
                    dvi = vadd(dvi, S[i])
                Sq = vsc(qd[i], S[i])
                dai = vadd(dai, mv(cr(dvi), Sq))
                if i == k and wrt == 'qd':
                    dai = vadd(dai, mv(cr(v[i]), S[i]))
                dv[i], da[i] = dvi, dai
                Gv, Gdv = mv(G[i], v[i]), mv(G[i], dvi)
                dF[i] = vadd(mv(G[i], dai), mv(cf(dvi), Gv))
                dF[i] = vadd(dF[i], mv(cf(v[i]), Gdv))
            for i in reversed(range(n)):   # 후방(조상 포함)
                if dF[i] is None:
                    continue
                D[i][k] = m.dot(S[i], dF[i])
                t = dF[i]
                if i == k and wrt == 'q':
                    t = vadd(t, mv(cf(S[i]), F[i]))
                p = par[i]
                if p >= 0:
                    t = mv(m.tr(X[i]), t)
                    if dF[p] is not None:
                        t = vadd(dF[p], t)
                    dF[p] = t
        out.append(D)
    return out

def aba_deriv(tree, q, qd, tau):
    """순동역학 도함수: −M⁻¹ ∂τ/∂q, −M⁻¹ ∂τ/∂q', M⁻¹"""
    n = len(q)
    qdd = m.aba(tree, q, qd, tau)
    Dq, Dv = rnea_deriv(tree, q, qd, qdd)
    H = m.crba(tree, q)
    Mi = [solve_lu(H, [float(i == j) for i in range(n)])[0]
          for j in range(n)]
    Mi = [list(r) for r in zip(*Mi)]
    neg = lambda A: [[-x for x in r] for r in A]
    return neg(m.matmul(Mi, Dq)), neg(m.matmul(Mi, Dv)), Mi

def euler_step(tree, x, tau, h):
    """세미 암시적 오일러: v+ = v + h q'', q+ = q + h v+"""
    n = len(tau)
    q, v = x[:n], x[n:]
    a = m.aba(tree, q, v, tau)
    v2 = [vi + h * ai for vi, ai in zip(v, a)]
    return [qi + h * vi for qi, vi in zip(q, v2)] + v2

def transition(tree, q, qd, tau, h):
    """A = ∂x+/∂x (해석적 도함수로 구성), B = ∂x+/∂τ"""
    n = len(q)
    aq, av, Mi = aba_deriv(tree, q, qd, tau)
    I = [[float(i == j) for j in range(n)] for i in range(n)]
    lin = lambda P, Q, s: [[p + s * x for p, x in zip(r, t)]
                           for r, t in zip(P, Q)]
    sc = lambda P, s: [[s * x for x in r] for r in P]
    Avv, Avq = lin(I, av, h), sc(aq, h)      # ∂v+/∂v, ∂v+/∂q
    Aqq, Aqv = lin(I, Avq, h), sc(Avv, h)    # ∂q+/∂q, ∂q+/∂v
    A = ([r1 + r2 for r1, r2 in zip(Aqq, Aqv)]
         + [r1 + r2 for r1, r2 in zip(Avq, Avv)])
    return A, sc(Mi, h * h) + sc(Mi, h)

① 정확도.

해석적 도함수와 이중수 자동 미분이 복소수 스텝과 $1.5\times10^{-14}$ 안에서 같다. 가장 어긋난 블록 $\partial\ddot q/\partial\dot q$는 $M^{-1}$(최대 성분 $3.7\times10^{3}$)을 곱하므로 상대 오차가 조건수만큼 커진 것이다 (추정). 중심 차분 $h=10^{-6}$은 $10^{-9}\sim10^{-6}$, 전진 차분 $h=10^{-8}$은 $10^{-7}\sim10^{-4}$이다. 복소수 스텝은 이 장의 기준값이므로 독립 검증으로 위 표의 해석적 도함수·자동 미분과의 일치와 라이브러리 네 가지의 일치를 사용한다.

② 차분 간격.

전진 차분은 $h=10^{-8}$에서 $2.0\times10^{-8}$이 최소이고 중심 차분은 $h=10^{-5}$에서 $1.2\times10^{-11}$이 최소이며, 더 작은 $h$에서는 반올림 오차가 커져 $h=10^{-12}$에서 $10^{-4}$에 이른다. 복소수 스텝은 $h\ge10^{-4}$에서 중심 차분과 같은 절단 오차($h=10^{-2}$에서 $1.7\times10^{-5}$)를 가지고 $h\le10^{-8}$에서는 $5\times10^{-16}$ 이하로 일정하며 $h=10^{-30}$에서도 $5\times10^{-16}$이다. 차분은 최적 $h$를 알아야 하고 그것도 함수마다 다르므로 6R 팔의 최적값을 다른 모델에 그대로 사용할 수 없다.

③ 비용.

해석적 도함수의 곱셈은 RNEA 1회의 $9.8$배($n=6$)에서 $64.6$배($n=48$)로 $n$에 거의 선형으로 늘어난다(전체 $O(n^{2})$). 중심 차분($4n$회 RNEA)은 $n$에 대해 같은 $O(n^{2})$이나 상수가 커서 해석적 구현이 $0.41\to0.34$배이다. 이 구현은 입력마다 부분 트리를 따로 전파하는 단순한 형태이며 공통 항을 재사용하지 않는다. 이중수와 복소수 스텝의 비용은 곱셈 횟수로 세지 않았고 각 방식이 $2n$회 실행이다(3.6.3.3).

④ 전이 행렬.

해석적 도함수로 만든 $A$·$B$가 복소수 스텝과 $4.4\times10^{-15}$ 안에서 같고 중심 차분과는 차분 오차($10^{-9}\sim2\times10^{-8}$)만큼 다르다.

⑤ 라이브러리 대조(실행 2026-11-05). 같은 6R 팔과 12관절 트리를 Pinocchio·MuJoCo(MJCF)·Drake(URDF)에 입력해 같은 상태에서 비교했다. Pinocchio 4.1.0의 computeRNEADerivatives·computeABADerivatives, MuJoCo 3.17.0의 mjd_inverseFD(eps $10^{-6}$, 전진 차분)·mjd_transitionFD(중심 차분, 오일러 적분기, $h=0.01$ s, 감쇠 0), Drake 1.58.0의 ToAutoDiffXd·CalcInverseDynamics·ExtractGradient, CasADi 3.8.2의 SX·jacobian(이 장의 3.5 코드를 추적)을 사용했다. 관절 순서는 이름으로 대응시켰다. 스크립트는 robotics/fig-src/3.6-libcheck.py·3.6-libcheck-casadi.py·3.6-libcheck-drake.py, 출력은 robotics/fig-src/data/3.6-libcheck-output.txt에 있다.

Pinocchio의 해석적 도함수가 이 장과 $1.6\times10^{-13}$ 이하, Drake의 자동 미분이 $1.1\times10^{-15}$ 이하, CasADi의 그래프 미분이 $1.7\times10^{-14}$ 이하에서 같다. MuJoCo는 유한 차분이어서 $\partial\tau/\partial q$에서 $8.2\times10^{-6}$ 차이가 나며(전진 차분의 오차로 예상되는 크기) 질량행렬 $M$($\partial\tau/\partial\ddot q$)은 $4\times10^{-9}$이다. mjd_inverseFD가 전치로 낸다는 점을 처음에 놓쳐 상대 오차 $1.0$·$2.0$이 나왔고 독스트링을 읽고 전치해 해결했다. mjd_transitionFD의 $A$는 이 장의 $A$와 $1.5\times10^{-8}$로 같다(상태 순서 $(q,v)$, 오일러 적분기에서). CasADi 표현식 노드 수는 6R 팔의 $\partial\tau/\partial(q,\dot q)$가 $5{,}702$, $\partial\ddot q$가 $14{,}488$이고 12관절 트리는 $16{,}618$·$40{,}398$이다.

Pinocchio의 해석적 computeRNEADerivatives는 rnea 1회의 $2.9$배($n=6$)에서 $5.0$배($n=96$)이고 파이썬 중심 차분 루프($4n$회 호출)보다 $11\sim66$배 빠르다. computeABADerivatives는 aba의 $1.7\sim10.6$배이고 차분 루프보다 $14\sim36$배 빠르다. 차분 루프는 파이썬 호출 비용이 포함되어 있어 차이가 과장되며, 값은 이 실행의 최솟값이고 컴퓨터·시점·다른 부하(실행 중 CPU를 공유했다)에 따라 달라진다. 이 장의 단순 해석적 구현(곱셈 횟수 RNEA의 $64.6$배, $n=48$)이 Pinocchio($n=48$에서 시간으로 RNEA의 $3.3$배)보다 훨씬 비싸다는 것은 두 값이 곱셈 횟수와 시간으로 지표가 달라 직접 비교할 수 없으나 알고리즘 효율의 차이를 시사한다 (추정).

한계. 회전 관절 6R 팔·트리·사슬에 한정하고 쿼터니언·부유 기저·직동 관절·접촉의 도함수는 시험하지 않았다. 이중수는 입력마다 한 번 실행하는 순방향 모드만 구현했고 역방향 모드는 CasADi로만 시험했다(CasADi jacobian이 내부에서 고르는 모드는 확인하지 않았다). Pinocchio·Drake·MuJoCo의 도함수 알고리즘 내부는 읽지 않았다.

견해: 도함수의 정확도가 필요한 DDP·MPC에서는 라이브러리의 해석적 도함수(Pinocchio)를 기본으로 두고, 해석적 도함수가 없는 모델·라이브러리(Drake의 MultibodyPlant·새 모델)에서는 자동 미분을 두며, 유한 차분은 최적 $h$를 모델마다 찾아야 하므로 검증용으로만 두는 것을 권한다고 본다. 근거는 이 장에서 ① 해석적·자동 미분이 반올림 수준($10^{-15}$)인 반면 중심 차분은 최선의 $h$에서도 $10^{-11}$이고 전진 차분은 $10^{-8}$이며, ② Pinocchio의 해석적 도함수가 파이썬 차분 루프보다 $n=96$에서 $66$배(RNEA)·$15$배(ABA) 빠르다는 점이다. 복소수 스텝은 구현이 간단하고 정확해 검증 기준값으로 사용했다. 이 비교는 회전 관절·한 컴퓨터에 한정되고 접촉이 있는 모델의 도함수는 비교하지 않았다.

3.6.6 설계 트레이드오프 — 도함수를 어떻게 얻을 것인가

• ① 유한 차분. 구현이 없고 어떤 시뮬레이터에도 적용된다. 간격 선택·오차 $10^{-8}\sim10^{-11}$, 비용 $2n\sim4n$회 평가. MuJoCo의 mjd_*가 이 방식이다.
• ② 복소수 스텝. 정확하고 구현이 간단하다. 복소수 입력을 받는 해석적 코드가 필요하고 비용이 입력당 복소수 평가 1회이다.
• ③ 자동 미분(순방향·역방향·그래프). 코드를 미분해 정확하다. 스칼라 형식을 바꿀 수 있는 라이브러리(Drake·CasADi)가 필요하고 순방향은 입력 수에 비례한다.
• ④ 해석적 도함수. 정확하고 가장 빠를 수 있다. 유도·구현·검증 부담이 크다. Pinocchio가 RNEA·ABA에 제공한다.

견해: 새 모델에서 도함수를 처음 구현할 때는 ③(또는 ②)을 먼저 작성하고, 병목이 확인되면 라이브러리의 ④로 바꾸며 ②를 회귀 시험의 기준값으로 남긴다고 본다. 근거는 이 장에서 ②와 ④가 $1.5\times10^{-14}$ 이하로 일치해 ②가 검증 기준으로 충분했다는 점이다. 구현 시간·유지 비용은 이 장이 측정하지 않았다.

3.6.7 응용 축 비교

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

3.6.8 관련 규격·프로토콜

동역학 도함수를 규정한 표준은 확인하지 못했다 (확인 필요).

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

동역학 도함수·라이브러리를 규정하는 법령·인증 요건은 확인하지 못했다. 간접 연결은 모델 기반 제어기의 안전성 검증에서 사용하는 선형화·안정 여유 계산이 도함수의 정확도에 의존한다는 점이다(12.2). 이 장은 규정의 조항을 확인하지 않았다 (확인 필요).

3.6.10 한계·유의점·미해결

• 모델 범위. 회전 관절 6R 팔·트리·사슬이다. 쿼터니언·부유 기저·직동·구면 관절, 접촉·마찰·유연 요소의 도함수는 시험하지 않았다.
• 효율적 알고리즘 부재. 이 장의 해석적 구현은 입력마다 부분 트리를 전파하는 단순한 형태이며 Carpentier·Mansard(2018)의 효율적 알고리즘과 비교하지 못했다 (확인 필요).
• 자동 미분의 범위. 순방향 이중수와 CasADi 그래프 미분, Drake AutoDiffXd만 시험했다. 역방향 모드의 비용·메모리와 Pinocchio의 CasADi·CppAD 연결은 시험하지 못했다.
• 시간 측정. 시간은 한 컴퓨터·CPU 공유 상태의 최솟값이며 재현 시 달라진다. 파이썬 호출 비용이 차분 루프에 포함된다.
• MuJoCo. 유한 차분의 오차는 eps와 중심 여부에 의존하고 접촉이 있는 모델은 시험하지 않았다.
• RBDL. 2.6.0 소스에서 도함수 함수를 찾지 못한 것이며 다른 판·확장에는 있을 수 있다 (확인 필요).

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

체크포인트:

1. $\partial\ddot q/\partial q=-M^{-1}\partial\tau/\partial q$를 $\tau=\mathrm{RNEA}(q,\dot q,\ddot q)$의 전미분에서 유도하라.
2. 전진 차분·중심 차분의 최적 $h$를 오차식에서 유도하고 표의 최솟값과 비교하라.
3. 복소수 스텝에 반올림 오차가 없는 이유와 절단 오차가 중심 차분과 같은 이유를 설명하라.
4. $\partial X_i/\partial q_i=-(S_i\times)X_i$를 유도하고 후방 순환의 $S_i\times^{*}F_i$ 항의 부호를 확인하라.
5. 세미 암시적 오일러 전이 행렬 $A$·$B$를 유도하고 완전 암시적 오일러에서 어떻게 달라지는지 적어라.
6. 이중수 자동 미분의 $2n$회 실행과 역방향 모드 $n$회의 비용을 비교하고 $m\ll n$인 함수에서 유리한 모드를 논하라.
7. MuJoCo mjd_inverseFD의 전치 관례를 확인하는 시험을 설계하라.

연구 질문:

• 접촉이 있는 모델의 동역학 도함수(접촉 힘의 도함수)는 해석적·자동·유한 차분에서 어떻게 달라지는가(3.12·16.10)?
• 쿼터니언·부유 기저의 접선 공간 도함수는 라이브러리마다 어떤 관례를 사용하는가(3.13)?
• Carpentier·Mansard의 효율적 알고리즘과 이 장의 단순 알고리즘의 비용 차이는 구조(깊이·분기)에 어떻게 의존하는가?

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

• J. Carpentier, N. Mansard, "Analytical derivatives of rigid body dynamics algorithms," Robotics: Science and Systems XIV, 2018, doi:10.15607/RSS.2018.XIV.038 — 서지만 확인.
• J. Carpentier 외, "The Pinocchio C++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives," 2019 IEEE/SICE International Symposium on System Integration, 614–619, doi:10.1109/SII.2019.8700380 — 서지만 확인.
• W. Squire, G. Trapp, "Using complex variables to estimate derivatives of real functions," SIAM Review 40(1), 110–112, 1998, doi:10.1137/S003614459631241X — 서지만 확인.
• J. R. R. A. Martins, P. Sturdza, J. J. Alonso, "The complex-step derivative approximation," ACM Transactions on Mathematical Software 29(3), 245–262, 2003, doi:10.1145/838250.838251 — 서지만 확인.
• A. Griewank, A. Walther, Evaluating Derivatives, 2nd ed., SIAM, 2008, doi:10.1137/1.9780898717761 — 서지만 확인.
• J. A. E. Andersson 외, "CasADi: a software framework for nonlinear optimization and optimal control," Mathematical Programming Computation 11(1), 1–36, 2019, doi:10.1007/s12532-018-0139-4 — 서지만 확인.
• C. Mastalli 외, "Crocoddyl: An efficient and versatile framework for multi-contact optimal control," 2020 IEEE International Conference on Robotics and Automation, 2536–2542, doi:10.1109/ICRA40945.2020.9196673 — 서지만 확인.
• M. L. Felis, "RBDL: an efficient rigid-body dynamics library using recursive algorithms," Autonomous Robots 41(2), 495–511, 2017, doi:10.1007/s10514-016-9574-0 — 서지만 확인. E. Todorov, T. Erez, Y. Tassa, "MuJoCo: A physics engine for model-based control," IEEE/RSJ IROS 2012, 5026–5033, doi:10.1109/IROS.2012.6386109 — 서지만 확인.
• MuJoCo 문서 "Computation"(mujoco.readthedocs.io)과 mjd_inverseFD·mjd_transitionFD의 파이썬 독스트링, Pinocchio 4.1.0 독스트링, RBDL 2.6.0 소스 검색.
• Pinocchio 4.1.0, MuJoCo 3.17.0, Drake 1.58.0(PyPI drake), RBDL 2.6.0, CasADi 3.8.2, numpy 2.5.4.

다음 장. 3.7는 동역학 파라미터 식별(기저 파라미터·여기 궤적·물리적 타당성)을 다룬다. 이 장의 $\tau=Y\pi$와 도함수는 식별의 회귀 행렬·감도 분석으로 이어진다.