# 제3장 SO-101 기구학과 ROS 2 기반 하드웨어 텔레오퍼레이션 ## 장 개요 로봇팔 제어 소프트웨어는 세 가지 세계를 동시에 다룬다. 첫째는 링크와 관절로 구성된 기하학 모델, 둘째는 엔코더와 버스 서보가 있는 실제 장비, 셋째는 메시지와 시간으로 표현되는 분산 소프트웨어다. 어느 한쪽의 좌표축, 단위, 관절 순서가 어긋나도 가상 로봇은 그럴듯하게 움직이면서 실물은 반대 방향으로 돌 수 있다. 이 장에서는 SO-101을 사례로 기구학 사슬(kinematic chain)을 모델링하고, 순기구학(Forward Kinematics, FK)과 감쇠 최소제곱 역기구학(Damped Least Squares Inverse Kinematics, DLS IK)을 구현한다. 이어 Leader 암의 관절 상태를 Follower 암의 안전한 명령으로 변환하고, ROS 2 Jazzy의 `sensor_msgs/msg/JointState`를 이용해 실물과 시뮬레이터를 연결한다. SO-101은 자세를 만드는 5개의 팔 관절과 그리퍼 구동기 1개를 가진다. 따라서 모터는 6개지만, 그리퍼를 제외한 팔은 5-DoF이다. 3차원 위치 3개와 방향 3개를 동시에 임의로 지정하려면 일반적으로 6개의 독립 자유도가 필요하므로, SO-101의 역기구학은 위치 우선 또는 방향의 일부만 추종하는 식으로 과업 우선순위(task priority)를 정해야 한다. LeRobot의 공식 Isaac Teleop 설명도 SO-101을 5-DoF 팔로 다루며 방향 오차 가중치를 낮추는 위치 우선 IK를 사용한다.[Hugging Face, 2026] ![그림 3-1. SO-101의 5개 팔 관절과 그리퍼 구동기](images/so101_kinematic_chain.svg) *그림 3-1*은 교육용 개념도다. 정확한 축, 링크 오프셋, 한계는 사용 중인 URDF와 캘리브레이션 파일에서 읽어야 한다. ## 학습 목표 이 장을 마치면 학습자는 다음을 수행할 수 있다. 1. URDF의 `link`, `joint`, `origin`, `axis`, `limit`를 기구학 트리로 해석한다. 2. URDF 모델을 USD 물리 관절(articulation)로 옮길 때 보존해야 하는 정보를 설명한다. 3. 동차변환행렬과 DH(Denavit-Hartenberg) 파라미터로 순기구학을 계산한다. 4. 자코비안(Jacobian)과 DLS를 사용해 특이점 근처에서도 제한된 관절 증분을 계산한다. 5. Leader/Follower 캘리브레이션, 관절 순서, 부호, 단위를 일관되게 관리한다. 6. ROS 2 `JointState` 토픽을 이용해 실물과 시뮬레이터를 동기화한다. 7. 관절 제한, 속도 제한, 워치독(watchdog), 데드맨 스위치(dead-man switch), 비상 정지를 계층적으로 설계한다. ## 선수 지식과 준비물 ### 선수 지식 - Python, NumPy 및 C++17 기초 - 4×4 동차변환행렬과 회전행렬 - ROS 2 노드, 토픽, QoS(Quality of Service)의 기본 개념 - Linux 직렬 장치와 사용자 권한의 기본 개념 ### 기준 환경 - Ubuntu 24.04 LTS - ROS 2 Jazzy Jalisco - Python 3.12, NumPy - C++17, `rclcpp`, `sensor_msgs` - LeRobot 최신 안정 릴리스: CLI와 설정 필드는 릴리스에 따라 바뀔 수 있으므로 실행 전 공식 SO-101 문서를 확인한다. - 선택: SO-101 Leader/Follower, 각 암의 전원 공급 장치와 USB 직렬 어댑터 - 선택: NVIDIA Isaac Sim과 SO-101 URDF/USD 자산 ### 실물 실습 전 안전 원칙 > **위험:** 텔레오퍼레이션 코드는 사람의 손동작을 즉시 모터 명령으로 바꾼다. 잘못된 관절 부호, 첫 프레임의 큰 점프, 통신 지연은 충돌과 끼임을 일으킬 수 있다. 실물 전원을 넣기 전에 다음을 확인한다. 1. 암을 단단한 작업대에 고정하고 예상 작업 공간에서 사람, 케이블, 공구를 치운다. 2. 모터와 전원 공급 장치의 정격 전압이 일치하는지 확인한다. 변형별 정격이 다를 수 있다. 3. 하드웨어 전원 차단 수단을 조작자의 손이 닿는 곳에 둔다. 소프트웨어의 `Ctrl+C`는 비상 정지가 아니다. 4. 최초 시험은 토크 비활성 또는 저속·무부하 조건에서 수행한다. 5. 관절별 소프트 리밋(software limit)을 실제 기계 한계보다 안쪽에 둔다. 6. Leader와 Follower의 첫 자세가 크게 다르면 즉시 미러링하지 말고 제한된 속도로 정렬한다. 7. 명령 스트림이 끊기면 현재 자세 유지 또는 토크 해제 중 장비에 맞는 안전 상태로 전이한다. ## 3.1 SO-101 기구학 모델 ### 3.1.1 링크, 관절, 구동기의 구분 URDF(Unified Robot Description Format)는 로봇을 링크(link)와 관절(joint)의 트리로 표현한다. SO-101의 일반적인 의미론적 관절 순서는 다음과 같다. | 순서 | 의미 | 대표 이름 | 역할 | |---:|---|---|---| | 1 | 베이스 회전 | `shoulder_pan` | 작업 공간의 방위각 변경 | | 2 | 어깨 굽힘 | `shoulder_lift` | 팔의 높이와 반경 변경 | | 3 | 팔꿈치 굽힘 | `elbow_flex` | 도달 거리 변경 | | 4 | 손목 피치 | `wrist_flex` | 그리퍼 접근 각도 변경 | | 5 | 손목 롤 | `wrist_roll` | 그리퍼 축 주위 회전 | | 6 | 그리퍼 | `gripper` | 손가락 개폐, 팔의 pose DoF에는 미포함 | 이름은 LeRobot/URDF 버전에 따라 다를 수 있다. 이름을 정수 인덱스로 암묵적으로 치환하지 말고, 항상 메시지의 `name` 배열을 기준으로 재정렬한다. `JointState`의 `position[i]`, `velocity[i]`, `effort[i]`는 같은 인덱스의 `name[i]`에 대응하며, 각 배열은 비어 있을 수 있다. 타임스탬프는 모든 관절 값이 획득된 시각 또는 가능한 한 가까운 시각을 나타내야 한다.[ROS 2 `JointState` 명세] ### 3.1.2 URDF에서 읽어야 할 정보 회전 관절의 핵심 URDF 요소는 다음과 같다. ```xml ``` - `origin`은 부모 링크에서 관절 프레임으로 가는 고정 변환이다. - `axis`는 관절 프레임에서 표현된 회전축이다. - `lower`, `upper`는 radian 단위의 위치 한계다. - `velocity`와 `effort`는 허용 가능한 변화율과 힘/토크 한계를 표현한다. 공식 SO-ARM100 저장소는 새로운 캘리브레이션 모델에서 관절 가상 영점(virtual zero)을 범위 중앙에 두며, 과거 모델은 팔을 수평으로 편 자세를 영점으로 사용한다. 서로 다른 영점 규약의 URDF와 캘리브레이션 JSON을 섞으면 같은 숫자가 다른 실제 자세를 의미한다.[TheRobotStudio, 2026] ### 3.1.3 URDF에서 USD Articulation으로 OpenUSD의 `UsdPhysicsArticulationRootAPI`는 여러 강체와 관절을 축약 좌표(reduced coordinate) 관절계로 묶는다. 회전 관절은 축, 상·하한, 위치/속도 드라이브의 강성(stiffness)과 감쇠(damping)를 가진다. URDF를 Isaac Sim으로 가져올 때 다음을 검토한다. 1. **단위:** URDF 길이는 관례상 m, 각도는 radian이다. USD 스테이지의 `metersPerUnit`, 회전 관절 한계의 단위와 임포터 변환을 확인한다. 2. **축과 부호:** 화면에서 회전 방향만 보고 맞다고 판단하지 말고, 작은 양의 관절 명령으로 축을 검증한다. 3. **루트:** 고정 베이스인지 부유 베이스(floating base)인지 명시한다. 4. **질량과 관성:** 시각 메시만 있는 링크에 임의의 물리 질량이 생성되지 않았는지 확인한다. 5. **충돌 형상:** 시각 메시와 충돌 메시를 분리한다. 복잡한 삼각 메시를 동적 충돌체로 그대로 쓰지 않는다. 6. **드라이브:** 너무 큰 stiffness는 수치적으로 불안정하거나 실물보다 과도하게 단단한 응답을 만든다. 7. **Mimic/그리퍼:** 단일 구동기로 양쪽 손가락을 묶는 관계가 임포트 후 유지되는지 확인한다. USD Physics 명세는 관절계 루트, 관절, 강체 목록으로 articulation을 구성하며 회전 관절 한계와 드라이브를 명시한다.[OpenUSD, 2026] ![그림 3-2. URDF에서 USD articulation으로 보존해야 할 정보](images/urdf_to_usd.svg) ## 3.2 순기구학 ### 3.2.1 동차변환 체인 관절 벡터를 $q=[q_1,\ldots,q_n]^T$라 하자. 베이스 프레임 $\{0\}$에서 엔드이펙터 프레임 $\{E\}$까지의 변환은 $$ {}^{0}T_E(q) = {}^{0}T_1(q_1){}^{1}T_2(q_2)\cdots{}^{n-1}T_E(q_n) $$ 이다. 변환행렬은 $$ T=\begin{bmatrix}R & p\\0&1\end{bmatrix}, $$ 여기서 $R\in SO(3)$는 회전, $p\in\mathbb{R}^3$는 위치다. 행렬 곱은 교환법칙이 성립하지 않으므로 관절 순서가 곧 모델이다. ### 3.2.2 표준 DH 표현 표준 DH 파라미터 $(a_i,\alpha_i,d_i,\theta_i)$의 한 링크 변환은 $$ A_i = R_z(\theta_i)T_z(d_i)T_x(a_i)R_x(\alpha_i) $$ 이며, 회전 관절에서는 $\theta_i=q_i+\theta_{0,i}$가 된다. DH는 계산과 교육에 편리하지만, URDF의 임의 축·오프셋을 변환하는 과정에서 실수가 생길 수 있다. 실무에서는 다음 두 모델을 나란히 둔다. - **권위 모델(authoritative model):** 버전을 고정한 URDF와 캘리브레이션 파일 - **계산 모델(computational model):** DH 또는 Product of Exponentials로 정리한 코드 무작위 관절 자세 여러 개에서 두 모델의 엔드이펙터 위치와 회전을 비교해야 한다. 이 장의 `code/fk_dls_ik.py`는 알고리즘 학습용 5축 DH 예제이며, 실제 SO-101 치수를 주장하지 않는다. ### 3.2.3 기하 자코비안 베이스 좌표계에서 관절 $i$의 회전축을 $z_{i-1}$, 관절 원점을 $o_{i-1}$, 엔드이펙터 원점을 $o_E$라 하면 회전 관절의 자코비안 열은 $$ J_{v,i}=z_{i-1}\times(o_E-o_{i-1}),\qquad J_{\omega,i}=z_{i-1}. $$ 따라서 $$ \begin{bmatrix}v_E\\\omega_E\end{bmatrix}=J(q)\dot q. $$ 5축 팔의 전체 pose 자코비안은 $6\times5$다. 모든 6차원 twist를 독립적으로 만들 수 없으므로 위치만 사용하는 $3\times5$ 자코비안이나 방향 오차에 낮은 가중치를 둔 가중 자코비안을 선택한다. ## 3.3 DLS 역기구학 ### 3.3.1 선형화와 감쇠 현재 pose에서 작은 관절 변화 $\Delta q$에 대한 작업 공간 변화는 $$ \Delta x\approx J(q)\Delta q $$ 이다. 단순 의사역행렬은 특이값이 0에 가까울 때 큰 관절 변화를 만들 수 있다. DLS는 $$ \min_{\Delta q}\;\|J\Delta q-e\|^2+\lambda^2\|\Delta q\|^2 $$ 를 풀어 다음 갱신을 얻는다. $$ \Delta q=J^T(JJ^T+\lambda^2I)^{-1}e. $$ $\lambda$가 커지면 관절 변화는 작아지지만 목표 추종 오차가 늘어난다. Wampler는 자코비안 기반 IK가 특이점에서 겪는 문제를 DLS 문제로 재정식화해 모든 자세에서 유용한 보정량을 얻는 방법을 제시했다.[Wampler, 1986] ::: {.learning-aid} **이해 도움말 — DLS의 감쇠는 ‘브레이크’에 가깝다** 특이점 근처에서는 엔드이펙터를 아주 조금 움직이기 위해 일부 관절을 매우 크게 움직여야 한다. 이때 $\lambda$는 관절 증분이 폭주하지 않도록 제동을 건다. $\lambda$가 작으면 목표를 더 적극적으로 추종하지만 노이즈와 모델 오차에도 민감하고, 크면 움직임은 안정적이지만 목표에 덜 정확하게 접근한다. 따라서 $\lambda$는 “정확도 설정값”이 아니라 **추종 오차와 관절 움직임 사이의 절충(trade-off)**이다. ::: ### 3.3.2 반복 알고리즘 1. 현재 $q_k$에서 FK로 $x_k$를 구한다. 2. 오차 $e_k=x^*-x_k$를 계산한다. 3. $J(q_k)$와 DLS 증분 $\Delta q$를 구한다. 4. $\|\Delta q\|_\infty$를 한 스텝 최대값으로 제한한다. 5. $q_{k+1}=\operatorname{clip}(q_k+\Delta q,q_{min},q_{max})$로 관절 한계를 적용한다. 6. 허용 오차, 최대 반복 수, 진전 부족(stagnation)을 검사한다. `np.linalg.inv`로 역행렬을 직접 만들지 말고 `np.linalg.solve`를 사용한다. 목표가 작업 공간 밖이면 “가장 가까운 자세”가 성공처럼 보일 수 있으므로 수렴 여부와 최종 오차를 함께 반환한다. ### 3.3.3 5-DoF 과업 설계 SO-101에 6차원 pose 오차를 같은 가중치로 강제하면 서로 양립할 수 없는 목표 때문에 진동할 수 있다. 다음 전략을 선택할 수 있다. - 위치 3차원만 정확히 추종하고 손목 각도는 별도 명령한다. - $W=\operatorname{diag}(1,1,1,w_r,w_r,w_r)$에서 $0