Core robotic fundamentals

두 관절 로봇으로 보는 Body Schema Policy

정기구학과 Jacobian을 두 관절 로봇으로 복습하고, 같은 로봇에서 Body Schema Policy의 계산을 한 단계씩 따라갑니다.

PART 1. 기구학 복습

1. 예제 로봇

이 글 전체에서 로봇 하나만 씁니다. 링크 3개(몸통, 위팔, 아래팔)와 회전 관절 2개(어깨, 팔꿈치)로 이루어진 평면 로봇입니다. 위팔과 아래팔의 길이는 $l_1 = l_2 = 1$이고, 어깨 각도는 $q_1$, 팔꿈치 각도는 $q_2$, 손의 위치는 $(x, y)$입니다.

x y 어깨 팔꿈치 손
손 위치
Jacobian $J$
$\det J$
$J^\top F$

화살표와 $J$, $J^\top F$의 뜻은 7절에서 설명합니다. 주황색 화살표는 어깨만 돌 때, 파란색 화살표는 팔꿈치만 돌 때 손의 속도입니다. 점선 원은 손이 닿을 수 있는 범위입니다. $J^\top F$는 손에 아래로 1 N이 걸릴 때의 관절 토크입니다.

2. 강체 변환과 4×4 행렬

강체의 자세는 회전 $R \in SO(3)$와 위치 $p \in \mathbb{R}^3$로 정해지고, 둘을 4×4 행렬 하나로 씁니다. 강체 변환의 정의는 앞선 글에 있습니다.

$$T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \in SE(3)$$

이 표현에서 기억할 것은 두 가지입니다.

확인

$T_1 T_2$의 위치 부분은 무엇입니까? 답: $R_1 p_2 + p_1$입니다. 둘째 변환의 위치가 첫째 변환의 회전을 거친 뒤 더해집니다.

3. 회전의 지수 표현과 Rodrigues 공식

단위 벡터 $\omega$를 축으로 각도 $\theta$만큼 도는 회전은 $R = \exp(\hat\omega \theta)$로 씁니다. $\hat\omega$는 $\hat\omega x = \omega \times x$가 되도록 만든 3×3 반대칭 행렬입니다.

$$\hat\omega = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix}$$

행렬 지수는 무한급수이지만, $\hat\omega^3 = -\hat\omega$라는 성질 덕분에 닫힌 형태로 정리됩니다. 이것이 Rodrigues 공식입니다.

$$\exp(\hat\omega \theta) = I + \sin\theta\, \hat\omega + (1 - \cos\theta)\, \hat\omega^2 \tag{1}$$

$\omega = (0, 0, 1)$을 넣으면 익숙한 평면 회전이 나옵니다. 예제 로봇의 두 관절이 모두 이 축입니다.

$$\exp(\hat\omega \theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

식 (1)에서 눈여겨볼 점은 $\theta$가 $\sin$과 $\cos$ 안에만 나온다는 것입니다. 회전은 $1$, $\cos\theta$, $\sin\theta$ 세 함수의 조합으로 전부 표현됩니다. 14절에서 이 점이 다시 나옵니다.

확인

식 (1)에 $\theta = 0$을 넣으면 무엇입니까? 답: $I$입니다. 돌지 않았으므로 항등 변환입니다.

4. Screw 축

회전에 위치까지 합친 것이 screw 운동입니다. 관절 하나의 운동은 screw 축 $S = (\omega, v)$로 나타냅니다. $\omega$는 회전축이고 $v$는 선속도 성분입니다. 관절이 $\theta$만큼 움직일 때의 강체 변환은 다음과 같습니다($\lVert \omega \rVert = 1$일 때).

$$\exp(\hat S \theta) = \begin{bmatrix} \exp(\hat\omega \theta) & t(\theta) \\ 0 & 1 \end{bmatrix}, \qquad t(\theta) = \big(\theta I + (1 - \cos\theta)\, \hat\omega + (\theta - \sin\theta)\, \hat\omega^2\big)\, v \tag{2}$$

회전 부분은 식 (1) 그대로이고, 위치 부분 $t(\theta)$는 그 적분입니다. 관절의 종류에 따라 세 경우가 있습니다.

관절$S = (\omega, v)$$t(\theta)$
회전 관절, 축이 점 $r$을 지남$v = -\omega \times r$$(I - \exp(\hat\omega\theta))\, r$. $\sin$, $\cos$만 포함
직선 관절, 방향 $v$$\omega = 0$$\theta v$. $\theta$에 비례
나선 관절, 피치 $h$$v = -\omega \times r + h\omega$위의 회전 항에 $h\theta\omega$가 더해짐

예제 로봇의 두 관절은 다음과 같습니다. 어깨는 원점을, 팔꿈치는 기준 자세에서 $r = (1, 0, 0)$을 지납니다.

$$S_1 = \big((0,0,1),\ (0,0,0)\big), \qquad S_2 = \big((0,0,1),\ (0,-1,0)\big)$$

$S_2$의 $v$는 $-\omega \times r = -(0,0,1) \times (1,0,0) = (0,-1,0)$에서 나옵니다. 식 (2)에 넣으면 $t(\theta) = (1 - \cos\theta,\ -\sin\theta,\ 0)$입니다.

확인

회전 관절과 직선 관절의 $t(\theta)$는 $\theta$에 대해 어떻게 다릅니까? 답: 회전 관절은 주기 함수이고, 직선 관절은 $\theta$에 비례해서 계속 커집니다.

5. 정기구학

정기구학은 관절 각도에서 링크의 자세를 구하는 것입니다. 각 관절의 변환을 뿌리에서부터 차례로 곱하고, 마지막에 기준 자세 $M$을 곱합니다(지수 곱 공식). $M$은 모든 관절 각도가 0일 때 그 링크의 자세입니다.

$$P_{\text{손}} = \exp(\hat S_1 q_1)\, \exp(\hat S_2 q_2)\, M_{\text{손}} \tag{3}$$

같은 식을 링크를 하나씩 내려가는 재귀로 쓸 수 있습니다. $\tilde P_\ell$을 기준 자세를 곱하기 전까지 누적한 변환이라 하면,

$$\tilde P_{\text{몸통}} = I, \qquad \tilde P_{\text{위팔}} = \tilde P_{\text{몸통}}\, \exp(\hat S_1 q_1), \qquad \tilde P_{\text{아래팔}} = \tilde P_{\text{위팔}}\, \exp(\hat S_2 q_2), \qquad P_\ell = \tilde P_\ell\, M_\ell \tag{4}$$

식 (4)가 이 글에서 가장 중요한 식입니다. 자식 링크의 누적 변환은 부모 링크의 누적 변환에 관절의 강체 변환을 오른쪽에서 곱한 것입니다. PART 2의 BSP가 가져오는 것이 이 형태입니다.

숫자로 계산

$q_1 = 30^\circ$, $q_2 = 60^\circ$에서 손의 위치를 구합니다. 손의 기준 위치는 $(2, 0)$입니다. 평면이므로 $x$, $y$만 적습니다.

  1. 팔꿈치의 변환을 적용합니다. 회전은 $60^\circ$이고 $t = (1 - \cos 60^\circ,\ -\sin 60^\circ) = (0.5,\ -0.866)$입니다. $(2, 0)$을 $60^\circ$ 돌리면 $(1,\ 1.732)$이고, $t$를 더하면 $(1.5,\ 0.866)$입니다.
  2. 어깨의 변환을 적용합니다. 회전은 $30^\circ$이고 $t = 0$입니다. $(1.5,\ 0.866)$을 $30^\circ$ 돌리면 $(0.866,\ 1.5)$입니다.

식을 전개한 결과와 같습니다. 위 그림의 초기 상태에서 손 위치가 이 값입니다.

$$x = l_1 \cos q_1 + l_2 \cos(q_1 + q_2) = 0.866, \qquad y = l_1 \sin q_1 + l_2 \sin(q_1 + q_2) = 1.5 \tag{5}$$

확인

어깨만 $10^\circ$ 더 돌리면 아래팔의 누적 변환은 어떻게 됩니까? 답: 식 (4)에서 $\tilde P_{\text{위팔}}$이 바뀌므로 그것을 곱해 얻는 $\tilde P_{\text{아래팔}}$도 함께 바뀝니다. 한 관절의 움직임은 그 아래의 모든 링크에 전달됩니다.

6. 역기구학

역기구학은 반대 방향입니다. 손이 있어야 할 자세가 주어졌을 때 관절 각도를 구합니다. 정기구학은 식 (3)을 계산하면 끝이지만, 역기구학은 해가 여러 개이거나 없을 수 있습니다. 예제 로봇에서는 같은 손 위치에 대해 팔꿈치가 위로 꺾인 해와 아래로 꺾인 해, 두 개가 있습니다.

관절이 많아지면 닫힌 해를 구하기 어려워서, 현재 각도에서 조금씩 고쳐 가는 반복 계산을 씁니다. 그 계산에 필요한 것이 다음 절의 Jacobian입니다.

7. Jacobian

정의

정기구학을 함수 $x = f(q)$로 씁니다. 입력은 관절 각도 $q \in \mathbb{R}^n$이고 출력은 손의 위치(일반적으로는 자세) $x \in \mathbb{R}^m$입니다. Jacobian은 $f$의 각 출력을 각 입력으로 편미분해서 모은 $m \times n$ 행렬입니다.

$$J(q) = \frac{\partial f}{\partial q}(q) \in \mathbb{R}^{m \times n}, \qquad J_{ij}(q) = \frac{\partial f_i}{\partial q_j}(q) \tag{6}$$

$J_{ij}$는 관절 $j$를 조금 움직였을 때 출력의 $i$번째 성분이 변하는 비율입니다. 그래서 $J$는 현재 자세 $q$ 근처에서 정기구학을 1차로 근사한 것입니다.

$$f(q + \Delta q) \approx f(q) + J(q)\, \Delta q \tag{7}$$

식 (7)의 양변을 시간으로 나누면, 즉 $x(t) = f(q(t))$에 연쇄 법칙을 적용하면 관절 속도와 손 속도의 관계가 나옵니다.

$$\dot x = J(q)\, \dot q \tag{8}$$

$J$는 상수 행렬이 아니라 $q$의 함수입니다. 같은 로봇이라도 자세가 바뀌면 $J$가 바뀝니다.

예제 로봇에서

$f$는 식 (5)이고 $n = m = 2$입니다. 네 개의 편미분을 구합니다.

$$\frac{\partial x}{\partial q_1} = -l_1 \sin q_1 - l_2 \sin(q_1 + q_2), \qquad \frac{\partial x}{\partial q_2} = -l_2 \sin(q_1 + q_2)$$

$$\frac{\partial y}{\partial q_1} = l_1 \cos q_1 + l_2 \cos(q_1 + q_2), \qquad \frac{\partial y}{\partial q_2} = l_2 \cos(q_1 + q_2)$$

이것을 식 (6)대로 배열하면 다음과 같습니다.

$$J(q) = \begin{bmatrix} -l_1 \sin q_1 - l_2 \sin(q_1 + q_2) & -l_2 \sin(q_1 + q_2) \\ \phantom{-}l_1 \cos q_1 + l_2 \cos(q_1 + q_2) & \phantom{-}l_2 \cos(q_1 + q_2) \end{bmatrix} \tag{9}$$

$q_1 = 30^\circ$, $q_2 = 60^\circ$에서는 다음과 같습니다.

$$J = \begin{bmatrix} -1.5 & -1 \\ 0.866 & 0 \end{bmatrix}, \qquad \det J = l_1 l_2 \sin q_2 = 0.866$$

열의 의미

식 (8)에 $\dot q = (1, 0)$을 넣으면 $\dot x$는 $J$의 첫째 열이고, $\dot q = (0, 1)$을 넣으면 둘째 열입니다. 즉 $J$의 $j$번째 열은 관절 $j$ 하나만 1 rad/s로 돌 때 손이 움직이는 속도입니다. 1절 그림의 두 화살표가 이 두 열입니다. 위 숫자에서 둘째 열 $(-1,\ 0)$은 팔꿈치만 돌면 손이 $-x$ 방향으로 움직인다는 뜻입니다.

3차원에서

손의 위치만이 아니라 자세까지 다루면 출력은 6차원이고 $J$는 $6 \times n$입니다. 이때 $J \dot q$는 손의 각속도와 선속도를 함께 담은 twist입니다. 5절의 지수 곱 공식과 직접 이어지는 점이 있습니다. 공간 좌표계에서 쓴 Jacobian의 $j$번째 열은 관절 $j$의 screw 축을 현재 자세로 옮긴 것입니다. 첫째 열은 $S_1$ 그대로이고, 둘째 열은 $S_2$를 $\exp(\hat S_1 q_1)$만큼 옮긴 것입니다.

쓰임 1. 관절 속도에서 손 속도로

$\dot q = (0.1,\ 0.2)$ rad/s이면 $\dot x = J \dot q = (-0.35,\ 0.087)$입니다.

쓰임 2. 역기구학의 반복 계산

손을 $\Delta x$만큼 옮기는 데 필요한 관절 변화는 $\Delta q = J^{-1} \Delta x$입니다. 관절이 손의 자유도보다 많으면 의사역행렬 $J^{+}$를 씁니다. 목표 $x^\star$에 대해 $q \leftarrow q + J^{+}\big(x^\star - f(q)\big)$를 반복하는 것이 수치적 역기구학입니다. $f$는 정기구학입니다.

$$J^{-1} = \begin{bmatrix} 0 & 1.155 \\ -1 & -1.732 \end{bmatrix}, \qquad \Delta x = (0.01,\ 0) \ \Rightarrow\ \Delta q = (0,\ -0.01)$$

팔꿈치만 0.01 rad 펴면 손이 $x$ 방향으로 0.01 움직입니다. 이 자세에서 아래팔이 수직으로 서 있기 때문입니다.

쓰임 3. 손의 힘과 관절 토크

손에 힘 $F$가 걸리면 그것을 버티는 관절 토크는 $\tau = J^\top F$입니다. 유도는 일의 보존에서 나옵니다. 손이 $\delta x$만큼 움직일 때 힘이 하는 일 $F^\top \delta x$는 관절 토크가 하는 일 $\tau^\top \delta q$와 같아야 하고, 식 (7)에서 $\delta x = J\, \delta q$이므로 $F^\top J\, \delta q = \tau^\top \delta q$, 즉 $\tau = J^\top F$입니다.

같은 자세에서 손에 아래로 1 N이 걸리면,

$$\tau = J^\top F = \begin{bmatrix} -1.5 & 0.866 \\ -1 & 0 \end{bmatrix} \begin{bmatrix} 0 \\ -1 \end{bmatrix} = (-0.866,\ 0)$$

어깨에 0.866 N m가 걸리고 팔꿈치에는 걸리지 않습니다. 힘의 방향이 아래팔과 나란해서 팔꿈치에 대한 모멘트가 없기 때문입니다. 손에서 일어나는 접촉이 어느 관절에 얼마나 나타나는지는 자세에 따라 달라지고, 그것을 정하는 것이 $J^\top$입니다.

쓰임 4. 특이 자세

$\det J = l_1 l_2 \sin q_2$이므로 팔을 완전히 펴거나 접으면 $\det J = 0$입니다. 이때 $J$의 두 열이 같은 방향이 되어 손이 움직일 수 없는 방향이 생기고, $J^{-1}$이 발산합니다. 위 그림에서 $q_2$를 0으로 가져가면 두 화살표가 나란해집니다.

확인

위 그림에서 $q_1 = 0^\circ$, $q_2 = 90^\circ$로 두면 손에 걸리는 아래 방향 힘은 어느 관절에 나타납니까? 답: $J^\top F = (-1,\ 0)$입니다. 어깨에만 나타납니다. $q_2 = 0^\circ$로 펴면 $(-2,\ -1)$이 되어 두 관절 모두에 나타납니다.

PART 2. BODY SCHEMA POLICY

Body Schema Policy(BSP)는 휴머노이드 조작을 위한 diffusion policy입니다(arXiv 2611.22713). 식 (4)의 계산 형태를 신경망 안으로 가져온 것이 핵심입니다.

8. Diffusion policy 복습

Diffusion policy는 행동을 한 번에 내지 않고, 노이즈에서 시작해 여러 번 다듬어서 만듭니다. 행동은 앞으로 $H$스텝의 묶음 $a \in \mathbb{R}^{H \times A}$입니다. 예제 로봇에서는 두 관절의 목표 각도 16스텝이므로 $a \in \mathbb{R}^{16 \times 2}$, 숫자 32개입니다.

따라서 네트워크가 하는 일은 하나입니다. 노이즈가 섞인 행동 $a^{(k)}$, 단계 $k$, 관측을 받아 $\hat\epsilon$을 내는 것입니다. 이 네트워크를 denoiser라고 부릅니다. BSP는 이 denoiser의 내부를 설계한 것입니다.

9. BSP가 정기구학에서 가져온 것

BSP는 식 (4)의 계산 형태를 가져오고, 그 안에 들어가는 양을 전부 학습되는 것으로 바꿉니다. 실제 자세를 계산하지 않고, 같은 모양의 계산으로 특징을 전파합니다.

정기구학, 식 (4)BSP의 screw 층
링크가 갖는 것누적 변환 $\tilde P_\ell$, 4×4 행렬 1개특징 $L_\ell$, 4×4 행렬 16개. 자세가 아님
관절의 축로봇 설계에서 주어진 $S_d$학습되는 twist $\xi_d$. 주어지지 않음
관절의 각도측정된 $q_d$관절 특징에서 계산한 잠재 각도 $\theta_d$
부모에서 자식으로$\tilde P_{c} = \tilde P_{p}\, \exp(\hat S_d q_d)$$L_{c} \leftarrow \operatorname{LN}\big(L_{c} + L_{p}\, \exp(\hat\xi_d \theta_d)\big)$
주어지는 로봇 정보축, 링크 길이, 기준 자세어느 링크가 어느 링크에 붙어 있는지뿐
결과링크의 실제 자세행동 노이즈 예측에 쓰이는 조건 벡터

$\xi_d = (\omega, v)$는 4절의 screw 축과 같은 형태의 숫자 6개이고, $\exp(\hat\xi_d \theta_d)$는 식 (2)로 계산합니다. 다만 $\xi_d$는 학습으로 정해지는 가중치이고, $\theta_d$는 실제 관절 각도가 아닙니다. LN은 layer normalization입니다.

표의 screw 층 식은 요지만 적은 것입니다. 실제로는 관절마다 twist가 12개 있어서(오른쪽에서 곱하는 것 6개, 왼쪽에서 곱하는 것 6개) 16개 채널을 섞으며 전파하고, 블록 8개가 각자의 twist를 가집니다.

BSP는 Jacobian을 계산하지 않습니다. 쓰는 것은 정기구학의 곱셈 구조뿐입니다.

10. Denoiser 한 번의 계산

Denoiser는 노이즈가 섞인 행동을 받아 그 안의 노이즈를 맞히는 함수입니다. 예제 로봇에서 입력과 출력은 다음과 같습니다.

내용크기
입력노이즈가 섞인 행동 $a^{(k)}$. 어깨와 팔꿈치의 목표 각도 16스텝16 × 2
입력확산 단계 $k$1
입력현재 관절 각도 $s$2
입력영상패치 256개
출력노이즈 예측 $\hat\epsilon$. 좌표마다 16스텝16 × 2

계산은 네 단계입니다. 입력으로 관절과 링크의 특징을 만들고(단계 1), 그 특징을 몸의 연결 구조를 따라 갱신하고(단계 2, 3), 갱신된 특징에서 좌표별 조건을 읽어 노이즈를 예측합니다(단계 4).

기호

논문은 관절 특징을 $J$로 쓰지만, 이 글에서는 7절의 Jacobian과 구분하려고 $z$로 씁니다.

기호이름크기성격
$z_{\text{어깨}}$, $z_{\text{팔꿈치}}$관절 특징8입력마다 계산됨
$L_{\text{몸통}}$, $L_{\text{위팔}}$, $L_{\text{아래팔}}$링크 특징16 × 4 × 4입력마다 계산됨
$g$전역 토큰256입력마다 계산됨
$\theta_1$, $\theta_2$잠재 각도1입력마다 계산됨
$\xi_1$, $\xi_2$Twist6학습되는 가중치
$G_1$, $G_2$관절의 강체 변환4 × 4$\xi$와 $\theta$로 계산됨
$c_1$, $c_2$좌표별 조건128입력마다 계산됨

관절 특징은 관절에 하나씩, 링크 특징은 링크에 하나씩 있습니다. 어깨는 몸통(부모)과 위팔(자식)을 잇고, 팔꿈치는 위팔(부모)과 아래팔(자식)을 잇습니다. 관절 특징, 링크 특징, 전역 토큰을 합쳐 body schema라고 부릅니다.

단계 1. 초기화: 입력에서 특징을 만든다

$a^{(k)}$(숫자 32개), $k$의 임베딩(256개), $s$(2개)를 한 벡터로 이어 붙입니다. 이 벡터에 서로 다른 가중치 행렬 세 개를 곱해 관절 특징, 링크 특징, 전역 토큰을 각각 만듭니다.

$$\big[\operatorname{vec}(a^{(k)}),\ e(k),\ s\big] \ \longrightarrow\ \big(z_{\text{어깨}},\, z_{\text{팔꿈치}}\big),\ \ \big(L_{\text{몸통}},\, L_{\text{위팔}},\, L_{\text{아래팔}}\big),\ \ g$$

이 시점에는 몸의 구조가 아직 쓰이지 않습니다. 모든 특징이 같은 벡터에서 선형 변환으로 나옵니다. 중요한 점은 특징이 현재 관절 각도 $s$만이 아니라 행동 후보 $a^{(k)}$ 전체에서 만들어진다는 것입니다. 그래서 body schema는 "지금 몸이 어떤 자세인가"가 아니라 "지금 검토 중인 행동 후보"의 표현입니다.

단계 2. 블록: 부모에서 자식으로 전달한다 (8번 반복)

블록 하나는 세 가지 계산으로 이루어집니다. 아래 그림이 그 경로입니다.

몸통 특징 L4×4 행렬 16개 위팔 특징 L4×4 행렬 16개 아래팔 특징 L4×4 행렬 16개 G₁ 을 곱해 더함 (b) G₂ 를 곱해 더함 (b) 어깨 특징 z숫자 8개잠재 각도 θ₁ 을 냄 팔꿈치 특징 z숫자 8개잠재 각도 θ₂ 를 냄 (a) G₁ = exp(ξ₁ θ₁) (a) G₂ = exp(ξ₂ θ₂) (c) 위팔이 어깨를 갱신 (c) 아래팔이 팔꿈치를 갱신

(a) 관절이 강체 변환을 만든다. 각 관절이 자기 특징에서 스칼라 하나를 읽습니다. 이것이 잠재 각도입니다. 학습된 twist와 잠재 각도를 4절의 식 (2)에 넣어 4×4 강체 변환 행렬을 만듭니다.

$$\theta_1 = W_\theta\, z_{\text{어깨}} + b_\theta, \quad G_1 = \exp(\hat\xi_1 \theta_1), \qquad \theta_2 = W_\theta\, z_{\text{팔꿈치}} + b_\theta, \quad G_2 = \exp(\hat\xi_2 \theta_2)$$

(b) 부모 링크의 특징을 자식 링크로 전달한다. 부모 링크의 특징에 그 관절의 변환 행렬을 오른쪽에서 곱해 자식 링크의 특징에 더합니다. 5절의 식 (4)와 같은 모양입니다.

$$L_{\text{위팔}} \leftarrow \operatorname{LN}\big(L_{\text{위팔}} + L_{\text{몸통}}\, G_1\big), \qquad L_{\text{아래팔}} \leftarrow \operatorname{LN}\big(L_{\text{아래팔}} + L_{\text{위팔}}\, G_2\big)$$

(c) 자식 링크가 자기 관절을 갱신한다. 갱신된 자식 링크의 특징을 펴서 선형 변환한 뒤, 그 링크를 움직이는 관절의 특징에 더합니다.

$$z_{\text{어깨}} \leftarrow z_{\text{어깨}} + \operatorname{Lin}_1\big(\operatorname{vec}(L_{\text{위팔}})\big), \qquad z_{\text{팔꿈치}} \leftarrow z_{\text{팔꿈치}} + \operatorname{Lin}_2\big(\operatorname{vec}(L_{\text{아래팔}})\big)$$

한 블록 안에서 (b)의 두 식은 모두 그 블록에 들어온 값을 씁니다. 아래팔을 갱신할 때 쓰는 $L_{\text{위팔}}$은 같은 블록에서 방금 갱신된 값이 아니라 갱신 전의 값입니다. 그래서 정보는 블록 하나에 링크 한 칸씩 내려갑니다. 블록을 8번 반복하는 이유 중 하나입니다.

숫자로 보는 단계 2

(b)가 무엇을 하는지 보기 위해 가장 단순한 경우를 계산합니다. 다음과 같이 가정합니다. 실제 BSP에서는 이 가정들이 성립하지 않으며, 그 차이는 아래에서 다룹니다.

식 (2)로 두 변환을 계산합니다. 평면 운동이므로 회전각과 $x$, $y$ 이동만 적습니다.

$$G_1:\ \text{회전 } 0.5,\ \ t = (0,\ 0), \qquad G_2:\ \text{회전 } 1.0,\ \ t = (1 - \cos 1.0,\ -\sin 1.0) = (0.460,\ -0.841)$$

$L_{\text{몸통}}$$L_{\text{위팔}}$$L_{\text{아래팔}}$
처음$I$$0$$0$
블록 1 뒤$I$$0 + I\, G_1 = G_1$$0 + 0 \cdot G_2 = 0$
블록 2 뒤$I$$G_1 + I\, G_1 = 2 G_1$$0 + G_1 G_2 = G_1 G_2$

블록 1에서는 위팔만 갱신됩니다. 아래팔은 갱신 전의 $L_{\text{위팔}} = 0$을 쓰기 때문에 그대로입니다. 블록 2에서 아래팔에 $G_1 G_2$가 들어옵니다. 이것을 계산하면 회전 $1.5$ rad, 이동 $(0.807,\ -0.518)$입니다.

이 값은 5절의 식 (4)에서 $q_1 = 0.5$, $q_2 = 1.0$일 때 아래팔의 누적 변환 $\tilde P_{\text{아래팔}}$과 정확히 같습니다. 확인해 보면, 손의 기준 위치 $(2, 0)$에 이 변환을 적용한 결과가 $(0.948,\ 1.477)$이고, 식 (5)로 직접 계산한 $(\cos 0.5 + \cos 1.5,\ \sin 0.5 + \sin 1.5)$와 같습니다.

즉 위의 가정 아래에서는 screw 전파가 정기구학 그 자체입니다. 아래팔의 특징에 어깨의 각도와 팔꿈치의 각도가 곱의 형태로 함께 들어 있습니다. 실제 BSP는 여기서 세 가지가 다릅니다.

그래서 실제 BSP의 링크 특징은 자세가 아닙니다. 다만 위 계산이 보여 주듯이, 조상 관절들의 변환이 순서대로 곱해져 들어온다는 합성의 형태는 정기구학과 같습니다. BSP가 가져온 것은 이 형태입니다.

단계 3. 4번, 8번 블록에 추가되는 계산

4번째와 8번째 블록에서는 단계 2 뒤에 두 가지 계산이 더 있습니다.

영상은 이 경로로만 들어옵니다. 그 뒤에 일어나는 일이 중요합니다. 팔꿈치가 영상을 읽으면 $z_{\text{팔꿈치}}$가 바뀝니다. 다음 블록의 (a)에서 $\theta_2$가 달라지고, $G_2$가 달라지고, (b)에서 아래팔의 특징이 달라집니다. 영상에서 읽은 정보가 그 관절의 변환을 거쳐 그 아래 링크로 전달됩니다.

단계 4. 조건을 읽고 노이즈를 예측한다

8개 블록이 끝나면 행동 좌표마다 조건 벡터를 하나씩 읽습니다. 좌표는 관절과 일대일로 대응하므로, 그 관절의 특징과 부모 링크의 특징에서 읽습니다.

$$c_1 = W_c\big[z_{\text{어깨}},\, \operatorname{vec}(L_{\text{몸통}})\big], \qquad c_2 = W_c\big[z_{\text{팔꿈치}},\, \operatorname{vec}(L_{\text{위팔}})\big]$$

마지막으로 디코더 $f_\phi$가 노이즈를 예측합니다. 디코더는 1차원 합성곱 네트워크 하나이고, 두 좌표에 같은 가중치로 쓰입니다. 팔꿈치의 경우 입력은 팔꿈치 목표 각도의 노이즈 섞인 궤적 16개와 $c_2$이고, 출력은 그 궤적의 노이즈 16개입니다.

$$\hat\epsilon_{:,1} = f_\phi\big(a^{(k)}_{:,1} \mid c_1,\, e(k)\big), \qquad \hat\epsilon_{:,2} = f_\phi\big(a^{(k)}_{:,2} \mid c_2,\, e(k)\big)$$

두 결과를 합친 $\hat\epsilon \in \mathbb{R}^{16 \times 2}$가 denoiser의 출력입니다.

정리

단계하는 일몸의 구조가 쓰이는 방식
1. 초기화입력 전체에서 관절, 링크, 전역 특징을 만듦쓰이지 않음
2. 블록관절이 변환을 만들고, 부모 링크에서 자식 링크로 곱해서 전달연결 관계와 강체 변환의 곱
3. 4번, 8번 블록떨어진 부위의 참조, 관절별 영상 읽기관절마다 따로 영상을 읽음
4. 출력좌표별 조건을 읽고 좌표별로 노이즈를 예측좌표가 자기 관절의 특징만 조건으로 받음
확인

위의 숫자 예에서 블록을 한 번만 돌리면 아래팔의 특징에 어깨의 정보가 들어 있습니까? 답: 아닙니다. 블록 1 뒤의 $L_{\text{아래팔}}$은 0입니다. 어깨의 변환 $G_1$은 블록 1에서 위팔까지만 오고, 블록 2에서 아래팔에 도달합니다.

11. 학습되는 것과 계산되는 것

구분항목설명
학습되는 가중치$\xi_1$, $\xi_2$관절의 twist, 숫자 6개씩. 학습이 끝나면 고정
$W_\theta$, 초기화 변환, $\operatorname{Lin}_d$, attention, $W_c$, $f_\phi$, 영상 인코더나머지 가중치
입력마다 계산되는 값$z$, $L$, $g$관절 특징, 링크 특징, 전역 토큰
$\theta_1$, $\theta_2$잠재 각도. 실제 관절 각도가 아니고 감독도 없음
$G_1$, $G_2$, $c_1$, $c_2$강체 변환 행렬과 조건
정답이 주어지는 값$\epsilon$행동에 섞은 노이즈. 손실은 $\lVert \epsilon - \hat\epsilon \rVert^2$ 하나

Screw를 출력으로 예측하는 것이 아닙니다. 손실의 기울기가 디코더, 조건, 블록을 거슬러 올라가면서 $\xi$를 포함한 모든 가중치를 갱신합니다. 정기구학의 형태는 중간 계산을 그 모양으로 제한하는 데에만 쓰입니다.

잠재 각도 $\theta_d$는 식에서 관절 각도가 들어가던 자리에 들어가는 스칼라입니다. $\xi$를 2배로 하고 $\theta$를 절반으로 해도 같은 변환이 나오므로 값 자체에 물리적 단위는 없습니다. 실제 각도를 쓰지 않는 이유는, 관절 특징이 현재 각도가 아니라 16스텝의 행동 후보와 영상에서 읽은 정보를 담고 있기 때문입니다.

12. Body schema가 행동 denoising에 쓰이는 방식

디코더는 한 번에 좌표 하나의 궤적만 봅니다. 팔꿈치의 궤적을 처리할 때 디코더가 어깨의 계획, 현재 자세, 영상에 대해 아는 것은 $c_2$에 담긴 것이 전부입니다. 조건은 FiLM으로 들어갑니다. $c_n$에서 스케일 $\gamma$와 편향 $\beta$를 만들어 합성곱 특징에 $h \leftarrow \gamma \odot h + \beta$로 적용합니다.

따라서 body schema의 역할은 $c_n$을 만드는 것입니다. $c_2$에 정보가 들어가는 경로는 넷입니다.

경로$c_2$에 담기는 정보
초기화행동 후보 전체, 현재 관절 각도, 확산 단계
Screw 전파몸통과 어깨의 특징. $L_{\text{몸통}} G_1$을 거쳐 위팔로 들어옴
Self-attention트리에서 떨어진 링크의 특징
영상 cross-attention팔꿈치가 영상에서 읽은 정보

일반 네트워크에서는 좌표들의 정보가 제약 없이 섞입니다. BSP에서는 어깨의 정보가 팔꿈치의 조건에 들어가려면 $L_{\text{몸통}} G_1$이라는 강체 변환 곱을 거쳐야 합니다. 5절의 확인 문제에서 본 것, 즉 한 관절의 움직임이 그 아래 링크 전체에 전달된다는 의존 관계가 정보의 흐름에 그대로 걸려 있습니다.

13. 학습과 추론

학습. 시연에서 영상, $s$, $a$를 뽑고, $k$와 $\epsilon$을 뽑아 $a^{(k)}$를 만든 뒤, 10절의 계산으로 $\hat\epsilon$을 내고 $\lVert \epsilon - \hat\epsilon \rVert^2$를 줄입니다.

추론. 무작위 숫자 32개에서 시작합니다. 10절의 계산으로 $\hat\epsilon$을 얻어 노이즈를 줄이는 것을 10번 반복하면 행동 $a$가 나옵니다. 매 반복마다 특징이 그 시점의 행동 후보에서 다시 만들어지므로, 영상에서 읽는 위치도 반복마다 달라집니다. $a$의 16스텝을 실행한 뒤 다시 관찰합니다.

14. Rodrigues Network와의 차이

BSP의 바로 앞 연구인 Rodrigues Network도 기구학 트리를 따라 특징을 전파합니다. 차이는 관절의 변환을 어떻게 매개변수화하느냐에 있습니다.

논문은 관절 각도에서 링크 자세를 맞히는 정기구학 과제로 이 차이를 잽니다. 각 관절 범위의 가운데 절반에서 학습하고 전체 범위에서 평가했을 때, 이동하는 베이스가 있는 Fetch에서는 Rodrigues 연산의 오차가 $1.7 \times 10^7$배 컸고, 전부 회전 관절인 G1에서도 72배 컸습니다.

15. 실제 크기와 결과

실제 대상은 Unitree G1의 상체입니다. 예제 로봇과의 대응은 다음과 같습니다.

예제 로봇G1 상체
관절, 링크2개, 3개31개, 32개 (허리 3, 양팔 각 7, 양손 각 7)
행동16 × 216 × 36 (관절 목표 31, 몸통 높이와 이동 명령 5)
트리 밖의 좌표없음이동 명령 5개. 전역 토큰 $g$에서 조건을 읽고, 하체 제어기가 실행
크기35.04M (영상 인코더 DINOv2-S 22.06M, 나머지 12.98M)

SIMPLE 벤치마크의 G1 과제 9개에서, 과제당 시연 약 100개로 학습한 BSP는 270회 중 221회 성공했습니다. 약 75배 큰 Ψ0가 220회였고, 같은 크기로 맞춘 Patch Policy가 177회, Diffusion Policy가 111회였습니다.

논문의 분석 하나가 12절의 내용을 뒷받침합니다. 100단계 중 2단계만 지난 시점, 즉 행동 입력이 거의 순수한 노이즈일 때 BSP의 행동 추정 오차는 이미 0.05에서 0.08 사이였고, 같은 크기의 U-Net은 0.86에서 1.02 사이였습니다. 행동 입력에 정보가 거의 없는 시점이므로, 이 추정은 몸 상태와 영상이 body schema를 거쳐 만든 조건에서 나온 것입니다.