09 양팔 협력: 두 손이 어떻게 "심장이 하나 되어 움직이는가"
이전 글: 08_상태 추정과 양측 원격 조작.md는 로봇이 자신을 "인지하고 손끝의 감각을 전달하는 방법"을 다룬다. 본 글은 양팔 시나리오에서 독특한 세 가지 문제를 해결한다: ① 양손 간의 "상대적 운동"은 어떻게 기술하고 계산하는가? (상대 자세 → 상대 twist → 상대 제약행렬) ② 양손이 같은 물체를 잡고 있을 때, 힘은 어떻게 분배되어 "충돌하지 않게" 되는가? (폐쇄 체인 제약 + 내력) ③ 양팔 임피던스 제어가 왜 반드시 내력 항을 포함해야 하는가? (물체 임피던스 / 개별 임피던스 + 내력)
한 문장으로 정의하면: 양팔 협력 = "단일 팔의 운동학/역학" + "두 손 사이의 제약" —— 제약은 상대 자세에 숨어 있고, 힘은 내력에 있다. 스타일은 04-08과 동일: 각 공식은 어떻게 유도되었는지(추도) → 무엇을 의미하는지(직관) → 어떻게 사용하는지 → 실제 데이터로 설명한다. 관련 코드:
code/09_dual_arm.py는numpy만 의존하며,python3 code/09_dual_arm.py로 직접 실행 가능.
1. 양팔 ≠ 두 개의 단일 팔
1.1 두 가지 작업 모드
| 모드 | 목적 | 제약 |
|---|---|---|
| 독립 작업 | 왼손은 사과를 집고 오른손은 나사 돌리기, 각자 자기 일 하기 | 서로 충돌하지 않도록만 주의 |
| 협력 운반/조립 | 양손 함께 박스 들고 올리기, 보드 들어올리기, 구멍과 축 맞추기 | 양손 간 고정된 관계 (박스가 움직이지 않으면 양손도 움직이지 않음) |
독립 모드는 단순히 "두 개의 단일 팔"이며, 09 이전의 단일 팔 도구로 처리할 수 있다. 협력 모드가 09/10의 중심 —— 양손이 같은 물체를 잡으면 두 손은 그 물체에 "용접"되어 폐쇄 운동학 체인(closed-loop)이 되며, 제약은 "충돌 방지"에서 "위치 정확성"으로 진화한다.
비유 (반드시 기억해야 할 것):
- 독립 작업 = 두 사람이 각각 다른 상자를 실는 것, 충돌만 피하면 됨;
- 협력 운반 = 두 사람이 침대를 들고 다니는 것. 침대가 기울거나 떨어지면 안 됨 → 두 손은 반드시 "동기화"되어야 함, 한쪽이 빨라지면 다른 쪽도 따라가야 함.
"침대 들기"가 양팔 협력의 모든 비밀이다: 침대(물체)가 두 손을 하나로 묶어버림.
1.2 자유도 계산: 14개 관절, 왜 "결정권"은 8개?
양팔 7축은 2 × 7 = 14개의 관절을 가진다. 그러나 양손이 같은 강체를 잡고 있을 경우:
- 물체 자세 (위치 3 + 자세 3) = 6개의 자유도, 양손 위치 결정;
- 양손이 물체와 강성 연결 → 상대 자세 고정 → 6개 제약 (3D에서는 상대 자세가 6개 자유도로 구성되어 모두 고정됨);
- 남은 자유도: 14 − 6 = 8개 = 6(물체) + 2(내력 자유도).
14 ⏟ 관절 − 6 ⏟ 폐쇄 제약 = 6 ⏟ 물체 운동 + 2 ⏟ 내력 \underbrace{14}_{관절} - \underbrace{6}_{폐쇄 제약} = \underbrace{6}_{물체 운동} + \underbrace{2}_{내력} 관절 14−폐쇄 제약 6=물체 운동 6+내력 2
직관: 침대는 6개 자유도 (앞뒤, 좌우, 위아래, 기울기, 경사, 방향), 남은 2개 자유도는 어디로 갔는가? —— 두 사람이 서로 밀거나 당기는 힘(손과 손 사이 선 방향으로 압축 또는 인장)으로 변했다. 이 힘은 침대의 움직임을 만들지 않지만, 침대가 미끄러지거나 변형되지 않도록 결정하는 요소. 이것이 "내력"이며, 본 장 §6-7의 주요 주제.
2. 수학 기초: 자세, twist, Ad (초보자 10분 학습)
본 절은 이후 모든 공식의 "블록"이다. 01편에서 배웠다면 빠르게 건너뛰어도 좋지만, §2.3의 Ad는 꼭 읽어볼 것을 권함 —— 이전 버전 문서의 오류 원인.
2.1 자세 T: "위치 + 자세"의 묶음
기계 팔의 종단 자세는 4 × 4 4\times4 4×4 호이젠 행렬(3D) 또는 3 × 3 3\times3 3×3 행렬(2D 평면 단순화)로 표현된다:
T = [ R p 0 1 ] , R ⏟ 자세 (회전 행렬) , p ⏟ 위치 (원점 좌표) T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix},\qquad \underbrace{R}_{\text{자세 (회전 행렬)}},\qquad \underbrace{p}_{\text{위치 (원점 좌표)}} T=[R0p1],자세 (회전 행렬) R,위치 (원점 좌표) p
- T T T는 연쇄적으로 곱셈 가능 (먼저 { A } \{A\} {A}에서 { B } \{B\} {B}로 이동): T B W = T A W T B A T_B^{W} = T_A^{W} T_B^{A} TBW=TAWTBA.
- 상대 자세 T r e l = T L − 1 T R T_{rel} = T_L^{-1} T_R Trel=TL−1TR는 "왼손 좌표계에서 오른손 좌표계가 어디 있는지"를 나타낸다 (§3).
2.2 twist: 기계 팔 속도의 "정확한 표현"
기계 팔 종단이 움직이고 있을 때, 속도는 어떻게 표현할까? 초보자의 첫 번째 반응: 위치 p p p의 도함수 p ˙ \dot p p˙만 하면 될까?
문제: 평이 움직임은 문제 없지만, 자세는 그렇게 구할 수 없다. 회전 행렬 R R R의 "도함수" R ˙ \dot R R˙는 임의의 행렬이 아닌, R ˙ = ω ^ R \dot R = \widehat\omega\, R R˙=ω R(좌측 곱) 또는 R ˙ = R ω ^ \dot R = R\,\widehat\omega R˙=Rω (우측 곱)이어야 하며, 여기서 ω ^ \widehat\omega ω는 각속도 ω \omega ω의 "크로네커 곱 행렬"이다 (01편 참조). 따라서 각속도는 R ˙ R − 1 \dot R R^{-1} R˙R−1 또는 R − 1 R ˙ R^{-1}\dot R R−1R˙에 숨어 있으며, R ˙ \dot R R˙ 자체는 아님.
선 속도 v v v와 각속도 ω \omega ω를 결합하면 속도 스크롤(twist)이 된다:
V ⏟ twist (6차원) = [ v ω ] (3D) \underbrace{V}_{\text{twist (6차원)}} = \begin{bmatrix} v \\ \omega \end{bmatrix}\qquad\text{(3D)} twist (6차원) V=[vω](3D)
"어디서부터 추출하는지"에 따라 twist는 두 가지로 나뉜다 (면접에서 자주 출제, 명확히 구분해야 함):
| 정의 | 직관적인 의미 | 세계계 vs 본체계? | |
|---|---|---|---|
| 공간 twist V s V^s Vs | T ˙ T − 1 \dot T\, T^{-1} T˙T−1 | 물체가 지금 바로 세계 원점을 지나는 점의 속도 | 세계계 |
| 체 twist V b V^b Vb | T − 1 T ˙ T^{-1}\dot T T−1T˙ | 종단 좌표계 원점이 종단 자체에서 상대적으로 움직이는 속도 | 종단계 |
비유 (기차): 고속 열차 안에 있다.
- 공간 twist = 열차가 지면에 대해 이동하는 속도 (바깥에서 보았을 때, 열차가 지면 특정 지점을 지나는 속도);
- 체 twist = 차 내부에서 느끼는 속도 (차 내 탁자는 움직이지 않지만, 바깥은 빠르게 지나감).
기계 팔의 제약행렬 J ( q ) q ˙ = V J(q)\,\dot q = V J(q)q˙=V는 일반적으로 체 twist를 제공 (또는 공간 twist, 라이브러리마다 규칙 다름 —— 본 문서 코드는 모두 체 twist 사용, §5에서 다시 언급).
2.3 Ad: twist를 다른 좌표계로 "옮기는" 연산
두 좌표계의 twist를 서로 바꾸려면 어떻게 해야 할까? 답은 따라오는 행렬(adjoint) A d T Ad_T AdT이다:
V s = A d T V b , T = [ R p 0 1 ] V^s = \mathrm{Ad}_T\, V^b,\qquad T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} Vs=AdTVb,T=[R0p1]
유도 과정: 공간 twist와 체 twist의 정의는 "공역" 차이가 있다:
V s = T ˙ T − 1 = T ( T − 1 T ˙ ) ⏟ V b T − 1 = T V ^ b T − 1 V^s = \dot T\,T^{-1} = T\,\underbrace{(T^{-1}\dot T)}_{V^b}\,T^{-1} = T\, \widehat V^{\,b}\, T^{-1} Vs=T˙T−1=TVb (T−1T˙)T−1=TV bT−1
T V ^ b T − 1 T\,\widehat V^{\,b} T^{-1} TV bT−1을 전개 ( V ^ b = [ ω ^ v 0 0 ] \widehat V^b = \begin{bmatrix}\widehat\omega & v\\0&0\end{bmatrix} V b=[ω 0v0]):
T V ^ b T − 1 = [ R ω R T R v + p × ( R ω ) 0 0 ] T\,\widehat V^{\,b} T^{-1} = \begin{bmatrix} R\omega R^T & R v + p\times(R\omega) \\ 0 & 0 \end{bmatrix} TV bT−1=[RωRT0Rv+p×(Rω)0]
회전은 크로네커 곱 행렬을 바꾸지 않는다: R ω ^ R T = R ω ^ R\,\widehat\omega\,R^T = \widehat{R\omega} Rω RT=Rω . 따라서:
A d T V = [ R v + p × R ω R ω ] ⇒ A d T = [ R p ^ R 0 R ] (3D) \mathrm{Ad}_T\, V = \begin{bmatrix} Rv + p\times R\omega \\ R\omega \end{bmatrix} \quad\Rightarrow\quad \boxed{\mathrm{Ad}_T = \begin{bmatrix} R & \widehat p\,R \\ 0 & R \end{bmatrix}}\ \text{(3D)} AdTV=[Rv+p×RωRω]⇒AdT=[R0p RR] (3D)
2D 특례 (본 문서 코드에서 사용): ω \omega ω는 스칼라, p = ( p x , p y ) p=(p_x,p_y) p=(px,py), p × R ω = ω ( p y , − p x ) p\times R\omega = \omega(p_y,\ -p_x) p×Rω=ω(py, −px):
A d T = [ R [ p y − p x ] 0 1 ] 제3열 = ( p y , − p x ) ⏟ 주의! \mathrm{Ad}_T = \begin{bmatrix} R & \begin{bmatrix} p_y \\ -p_x \end{bmatrix} \\ 0 & 1 \end{bmatrix}\qquad \underbrace{\text{제3열 = }(p_y, -p_x)}_{\text{주의!}} AdT= R0[py−px]1 주의! 제3열 = (py,−px)
이전 문서의 함정은 여기 있었다: 이전 코드는 [ − p y , p x ] [-p_y,\ p_x] [−py, px]로 작성하여 부호가 반대였다. 어떻게 확인하나? 가장 기본적인 관계 V s = A d T V b V^s = \mathrm{Ad}_T\,V^b Vs=AdTVb: 같은 동작이면 두 측의 수치가 같아야 한다. 본 문서 §4에서는 이 관계를 수치 검증으로 다룰 예정이며, 부호 오류는 즉시 드러난다.
실제 데이터 대입 (코드 verify_adjoint 직접 출력): T = [ R ( 0.5 ) ( 1.0 , 0.5 ) 0 1 ] T=\begin{bmatrix}R(0.5) & (1.0, 0.5)\\0&1\end{bmatrix} T=[R(0.5)0(1.0,0.5)1], 체 twist V b = ( 1 , 0 , 0.3 ) V^b = (1,\ 0,\ 0.3) Vb=(1, 0, 0.3)일 때:
A d T = [ 0.878 − 0.479 0.5 0.479 0.878 − 1.0 0 0 1 ] , V s = A d T V b = [ 1.028 0.179 0.3 ] \mathrm{Ad}_T = \begin{bmatrix} 0.878 & -0.479 & 0.5\\ 0.479 & 0.878 & -1.0\\ 0&0&1 \end{bmatrix},\qquad V^s = \mathrm{Ad}_T V^b = \begin{bmatrix}1.028\\ 0.179\\ 0.3\end{bmatrix} AdT= 0.8780.4790−0.4790.87800.5−1.01 ,Vs=AdTVb= 1.0280.1790.3
항별 확인: R v = R ⋅ ( 1 , 0 ) = ( 0.878 , 0.479 ) Rv = R\cdot(1,0) = (0.878, 0.479) Rv=R⋅(1,0)=(0.878,0.479); ω ( p y , − p x ) = 0.3 ⋅ ( 0.5 , − 1.0 ) = ( 0.15 , − 0.30 ) \omega(p_y,-p_x) = 0.3\cdot(0.5,-1.0) = (0.15,-0.30) ω(py,−px)=0.3⋅(0.5,−1.0)=(0.15,−0.30); 더하면 ( 1.028 , 0.179 ) (1.028, 0.179) (1.028,0.179)입니다. ✓
직관: A d T Ad_T AdT는 = "자세를 먼저 옮기고(행렬 R 곱하기), 회전에 따른 추가 선속도(레버 암 항 p × R ω p\times R\omega p×Rω)를 보완하는 것". 이는 "강체의 두 점 속도 관계" v B = v A + ω × r v_B = v_A + \omega\times r vB=vA+ω×r와 동일한 물리 법칙(01편 참조).
3. 상대 자세 T r e l T_{rel} Trel: 양손 간의 "정렬 관계"
3.1 정의: 왜 T L − 1 T R T_L^{-1} T_R TL−1TR가 아니라 T R − T L T_R - T_L TR−TL인가?
자세는 "좌표계"이므로, 좌표계는 빼기 불가능하고 "변환"만 가능하다. 왼손 좌표계에서 오른손 자세:
T r e l = T L − 1 T R T_{rel} = T_L^{-1}\, T_R Trel=TL−1TR
유도 과정: 세계 좌표계에서 T R = T L ⋅ T r e l T_R = T_L \cdot T_{rel} TR=TL⋅Trel (먼저 왼손 좌표계로 이동 후 상대 관계 따라 오른손 좌표계로 이동), 양변에 T L − 1 T_L^{-1} TL−1를 곱하면 된다. T r e l T_{rel} Trel의 의미는: 왼손 종단 좌표계를 원점으로 삼고, 오른손은 어디에 있고 어떤 방향을 향하는가.
직관/용도:
- 조립 시: 양손이 부품에 맞춰야 하므로 T r e l T_{rel} Trel이 특정 설정값을 유지해야 함 (예: 구멍과 축이 일치 → T r e l T_{rel} Trel의 대각선은 0);
- 박스 들 때: T r e l T_{rel} Trel은 항상 상수여야 함 —— 박스가 기울거나 회전하지 않음 → T r e l T_{rel} Trel이 움직이지 않음.
3.2 실제 데이터 적용
평면 양팔 (연결부 길이 l = ( 1.0 , 0.8 , 0.4 ) l=(1.0, 0.8, 0.4) l=(1.0,0.8,0.4), 왼팔 기준 원점, 오른팔 기준 ( 1.5 , 0 ) (1.5, 0) (1.5,0)), 관절 각도 q L = ( 0.5 , 0.9 , − 0.2 ) q_L=(0.5, 0.9, -0.2) qL=(0.5,0.9,−0.2), q R = ( − 0.5 , 1.1 , 0.3 ) q_R=(-0.5, 1.1, 0.3) qR=(−0.5,1.1,0.3) (코드 _fk_planar 생성):
T L = [ 0.362 − 0.932 1.158 0.932 0.362 1.641 0 0 1 ] , T R = [ 0.622 − 0.783 3.286 0.783 0.622 0.286 0 0 1 ] T_L = \begin{bmatrix}0.362 & -0.932 & 1.158\\ 0.932 & 0.362 & 1.641\\ 0&0&1\end{bmatrix},\quad T_R = \begin{bmatrix}0.622 & -0.783 & 3.286\\ 0.783 & 0.622 & 0.286\\ 0&0&1\end{bmatrix} TL= 0.3620.9320−0.9320.36201.1581.6411 ,TR= 0.6220.7830−0.7830.62203.2860.2861
T r e l = T L − 1 T R = [ 0.955 0.296 − 0.492 − 0.296 0.955 − 2.474 0 0 1 ] T_{rel} = T_L^{-1} T_R = \begin{bmatrix}0.955 & 0.296 & -0.492\\ -0.296 & 0.955 & -2.474\\ 0&0&1\end{bmatrix} Trel=TL−1TR= 0.955−0.29600.2960.9550−0.492−2.4741
해석: 왼손 좌표계에서 보면, 오른손은 ( − 0.492 , − 2.474 ) (-0.492, -2.474) (−0.492,−2.474)에 위치하며, 약 atan2 ( − 0.296 , 0.955 ) ≈ − 0.30 \operatorname{atan2}(-0.296, 0.955)\approx -0.30 atan2(−0.296,0.955)≈−0.30 라디안 방향을 향하고 있음. 박스 들기란 이 행렬이 일정하게 유지되어야 한다.
4. 상대 twist: 양손 간의 "상대 속도"(핵심 공식, 중요 유도)
4.1 목표
상대 자세가 움직임(양손이 가까워지거나 회전함), 얼마나 빠르게 움직이는가? —— T r e l T_{rel} Trel의 도함수를 취하고 twist를 구하면 상대 twist V r e l V_{rel} Vrel가 된다. 이 값은 컨트롤러에게 "현재 양손의 상대 위치가 얼마나 빠르게 이동하는지"를 알려주며, 반대 방향으로 속도를 적용하면 T r e l T_{rel} Trel을 안정화할 수 있다.
4.2 유도: 단계별로
1단계: T r e l T_{rel} Trel의 도함수 작성. T r e l = T L − 1 T R T_{rel} = T_L^{-1} T_R Trel=TL−1TR, 곱의 법칙을 사용:
T ˙ r e l = ( T L − 1 ) ˙ T R + T L − 1 T ˙ R \dot T_{rel} = \dot{(T_L^{-1})}\, T_R + T_L^{-1}\,\dot T_R T˙rel=(TL−1)˙TR+TL−1T˙R
2단계: T L − 1 T_L^{-1} TL−1의 도함수 구하기. T L − 1 T L = I T_L^{-1} T_L = I TL−1TL=I 양변에서 도함수를 취하면:
0 = ( T L − 1 ) ˙ T L + T L − 1 T ˙ L ⇒ ( T L − 1 ) ˙ = − T L − 1 T ˙ L T L − 1 0 = \dot{(T_L^{-1})}T_L + T_L^{-1}\dot T_L \;\Rightarrow\; \dot{(T_L^{-1})} = -T_L^{-1}\dot T_L T_L^{-1} 0=(TL−1)˙TL+TL−1T˙L⇒(TL−1)˙=−TL−1T˙LTL−1
3단계: 대입 및 정리. T r e l = T L − 1 T R T_{rel} = T_L^{-1}T_R Trel=TL−1TR로 기억:
T ˙ r e l = − T L − 1 T ˙ L T L − 1 T R ⏟ T r e l + T L − 1 T ˙ R = T L − 1 ( T ˙ R − T ˙ L T r e l ) \dot T_{rel} = -T_L^{-1}\dot T_L\,\underbrace{T_L^{-1}T_R}_{T_{rel}} + T_L^{-1}\dot T_R = T_L^{-1}\big(\dot T_R - \dot T_L\, T_{rel}\big) T˙rel=−TL−1T˙LTrel TL−1TR+TL−1T˙R=TL−1(T˙R−T˙LTrel)
4단계: 체 twist 취하기 (왼쪽에 T r e l − 1 T_{rel}^{-1} Trel−1을 곱하여, V r e l b = T r e l − 1 T ˙ r e l V_{rel}^b = T_{rel}^{-1}\dot T_{rel} Vrelb=Trel−1T˙rel):
$$ V_{rel}^b = T_{rel}^{-1}\dot T_{rel} = \underbrace{T_R^{-1}\dot T_R}_{V_R^b}
- \underbrace{T_R^{-1}\dot T_L,T_L^{-1}T_R}{\text{왼손 기여도, 오른손 좌표계로 이동}} = V_R^b - \mathrm{Ad}{T_R^{-1}}, V_L^s $$
5단계: 공간 twist를 체 twist로 변환 (§2.3의 V L s = A d T L V L b V_L^s = \mathrm{Ad}_{T_L}V_L^b VLs=AdTLVLb):
A d T R − 1 V L s = A d T R − 1 A d T L V L b = A d T R − 1 T L V L b = A d T r e l − 1 V L b \mathrm{Ad}_{T_R^{-1}} V_L^s = \mathrm{Ad}_{T_R^{-1}}\mathrm{Ad}_{T_L} V_L^b = \mathrm{Ad}_{T_R^{-1}T_L}\, V_L^b = \mathrm{Ad}_{T_{rel}^{-1}}\, V_L^b AdTR−1VLs=AdTR−1AdTLVLb=AdTR−1TLVLb=AdTrel−1VLb
(Ad의 복합 성질: A d A A d B = A d A B \mathrm{Ad}_{A}\mathrm{Ad}_{B} = \mathrm{Ad}_{AB} AdAAdB=AdAB, A d T − 1 = A d T − 1 Ad_{T^{-1}} = Ad_T^{-1} AdT−1=AdT−1.)
결론 (기억할 것):
V r e l b = V R b − A d T r e l − 1 V L b \boxed{\,V_{rel}^b = V_R^b - \mathrm{Ad}_{T_{rel}^{-1}}\, V_L^b\,} Vrelb=VRb−AdTrel−1VLb
등가 형식 (면접에서 자주 출제, 세 가지 중 하나 선택):
| 형식 | 의미 |
|---|---|
| V r e l b = V R b − A d T r e l − 1 V L b V_{rel}^b = V_R^b - \mathrm{Ad}_{T_{rel}^{-1}} V_L^b Vrelb=VRb−AdTrel−1VLb | 체 twist, 왼손 좌표계에서 보기 (추천, 제약행렬 계산 편리) |
| V r e l b = A d T R − 1 ( V R s − V L s ) V_{rel}^b = \mathrm{Ad}_{T_R^{-1}}(V_R^s - V_L^s) Vrelb=AdTR−1(VRs−VLs) | 양손 공간 twist를 빼고 오른손 좌표계로 옮김 |
| V r e l s = A d T L − 1 V R s − V L b V_{rel}^s = \mathrm{Ad}_{T_L^{-1}}V_R^s - V_L^b Vrels=AdTL−1VRs−VLb | 공간(세계 좌표계) twist |
세 가지 형식은 어떻게 변환되는가: 같은 물리량이 서로 다른 좌표계/정의에서 다르게 표현될 때, Ad를 이용해 변환 가능. 문제/코드 작성 시에는 첫 번째 형식만 기억: 오른손의 체 속도에서 "왼손 속도를 오른손 좌표계로 옮긴 것"을 빼면 된다.
직관 (일상어): 두 사람이 침대를 들고 다니면, 당신(오른손)이 "침대가 당신에게 얼마나 빠르게 회전하는지" 알고 싶어할 때 = 당신 손에 느껴지는 속도 - 왼손 속도가 당신 손으로 이동했을 때의 영향. 왼손이 빠르면 당신은 이 속도가 얼마나 이동했는지도 알아야 하므로, 그렇지 않으면 양손이 서로 다른 방향으로 움직여 침대가 깨질 수 있다.
4.3 실제 데이터 검증 (이 숫자와 코드 verify_relative_jacobian는 완전히 일치)
§3.2의 팔과 관절 각도를 사용, 속도 ( q ˙ L , q ˙ R ) = ( 0.3 , − 0.4 , 0.2 , 0.1 , 0.5 , − 0.3 ) (\dot q_L,\dot q_R) = (0.3,-0.4,0.2,\ 0.1,0.5,-0.3) (q˙L,q˙R)=(0.3,−0.4,0.2, 0.1,0.5,−0.3) (rad/s):
V L b = ( 0.209 , 0.191 , 0.100 ) , V R b = ( 0.240 , 0.596 , 0.300 ) V_L^b = (0.209,\ 0.191,\ 0.100),\qquad V_R^b = (0.240,\ 0.596,\ 0.300) VLb=(0.209, 0.191, 0.100),VRb=(0.240, 0.596, 0.300)
V r e l b = V R b − A d T r e l − 1 V L b = ( − 0.154 , 0.325 , 0.200 ) (rad/s 항은 각속도) V_{rel}^b = V_R^b - \mathrm{Ad}_{T_{rel}^{-1}}V_L^b = (-0.154,\ 0.325,\ 0.200)\ \text{(rad/s 항은 각속도)} Vrelb=VRb−AdTrel−1VLb=(−0.154, 0.325, 0.200) (rad/s 항은 각속도)
항별 비교: 왼쪽에서 계산된 값은 T r e l T_{rel} Trel 수치 미분으로 추출한 체 twist와 완전히 일치 (오차 < 10 − 4 <10^{-4} <10−4, 코드에서 assert로 확인). 세 가지 등가 형식도 모두 동일. 이것이 "공식의 정확성" 증명 —— 그냥 복사한 것이 아니라 검증된 것.
5. 상대 제약행렬 J r e l J_{rel} Jrel: 관절 속도를 직접 상대 속도로 변환
5.1 정의 및 유도
§4의 V r e l b V_{rel}^b Vrelb는 양손 각각의 twist에 의존하며, twist는 관절 속도에 의해 결정된다. "관절 속도 → 상대 속도"의 선형 매핑을 상대 제약행렬로 표현한다.
단일 팔 (02편): V L b = J L ( q L ) q ˙ L V_L^b = J_L(q_L)\,\dot q_L VLb=JL(qL)q˙L, V R b = J R ( q R ) q ˙ R V_R^b = J_R(q_R)\,\dot q_R VRb=JR(qR)q˙R (체 twist 제약행렬). §4의 결론을 대입:
V r e l b = J R q ˙ R − A d T r e l − 1 J L q ˙ L = [ − A d T r e l − 1 J L J R ] [ q ˙ L q ˙ R ] V_{rel}^b = J_R\,\dot q_R - \mathrm{Ad}_{T_{rel}^{-1}}\, J_L\,\dot q_L = \begin{bmatrix} -\mathrm{Ad}_{T_{rel}^{-1}} J_L & J_R \end{bmatrix} \begin{bmatrix} \dot q_L \\ \dot q_R \end{bmatrix} Vrelb=JRq˙R−AdTrel−1JLq˙L=[−AdTrel−1JLJR][q˙Lq˙R]
J r e l = [ − A d T r e l − 1 J L J R ] ∈ R 6 × ( n L + n R ) \boxed{\,J_{rel} = \begin{bmatrix} -\mathrm{Ad}_{T_{rel}^{-1}} J_L & J_R \end{bmatrix}\ \in\ \mathbb{R}^{6\times (n_L+n_R)}\,} Jrel=[−AdTrel−1JLJR] ∈ R6×(nL+nR)
두 가지 강조해야 할 약속:
- J L , J R J_L, J_R JL,JR는 반드시 체 twist 제약행렬(관절 속도 → 체 twist)이어야 한다. 공간 제약행렬을 사용한다면, 먼저 변환 (02편 §3 참조).
- 이전의
Ad(T_rel) J_R − J_L형식은 잘못됨 (또한 q ˙ L = q ˙ R \dot q_L = \dot q_R q˙L=q˙R를 가정하며, 두 팔이 정확히 같은 방식으로 움직여야 하므로, 당연히 틀림). 올바른 형식은 "블록화": 왼쪽 블록은 부호를 반전하고 오른손 좌표계로 이동, 오른쪽 블록은 그대로.
직관: J r e l J_{rel} Jrel의 L L L 열은 "왼손 관절이 한 번 움직였을 때, 상대 속도가 얼마나 변화하는가"를 나타낸다 (Ad를 통해 오른손 좌표계로 이동하고 부호 반전 —— 왼손 움직임 = 상대 관계 반대 움직임); R R R 열은 "오른손 관절이 한 번 움직였을 때, 상대 속도가 얼마나 변화하는가" (그 자리에서 계산, 부호 유지).
5.2 용도 (왜 이것이 양팔 제어의 "方向盘"인가)
- 조립 맞춤: 목표 = V r e l V_{rel} Vrel이 특정 방향으로 수렴하도록 → J r e l † V r e l d e s J_{rel}^\dagger V_{rel}^{des} Jrel†Vreldes로 양팔 관절 속도 계산;
- 박스 들기: 목표 = V r e l d e s = 0 V_{rel}^{des} = 0 Vreldes=0 → 양손이 자동으로 "상대 위치 고정";
- 중복 분배: J r e l J_{rel} Jrel의 영공간은 "상대 관계를 변경하지 않는" 관절 운동 → 충돌 회피, 관절 제한 회피에 활용 (02편 영공간 기법과 동일, 다만 제약은 "종단 고정"에서 "상대 고정"으로 변경).
5.3 실제 데이터 검증
§4.3의 설정을 사용하여 수치적으로 체 제약행렬을 구하고 공식에 따라 J r e l J_{rel} Jrel 계산 (3 × 6 3\times6 3×6, 평면):
J r e l = [ − 2.397 − 2.007 − 2.391 1.222 0.236 0 − 2.267 − 1.346 − 0.644 1.334 1.164 0.4 − 1 − 1 − 1 1 1 1 ] J_{rel} = \begin{bmatrix} -2.397 & -2.007 & -2.391 & 1.222 & 0.236 & 0\\ -2.267 & -1.346 & -0.644 & 1.334 & 1.164 & 0.4\\ -1 & -1 & -1 & 1 & 1 & 1 \end{bmatrix} Jrel= −2.397−2.267−1−2.007−1.346−1−2.391−0.644−11.2221.33410.2361.164100.41
( q ˙ L , q ˙ R ) = ( 0.3 , − 0.4 , 0.2 , 0.1 , 0.5 , − 0.3 ) (\dot q_L, \dot q_R) = (0.3,-0.4,0.2, 0.1,0.5,-0.3) (q˙L,q˙R)=(0.3,−0.4,0.2,0.1,0.5,−0.3) 대입:
J r e l [ q ˙ L q ˙ R ] = ( − 0.154 , 0.325 , 0.200 ) J_{rel}\,\begin{bmatrix}\dot q_L\\\dot q_R\end{bmatrix} = (-0.154,\ 0.325,\ 0.200) Jrel[q˙Lq˙R]=(−0.154, 0.325, 0.200)
§4.3에서 직접 수치 미분으로 얻은 V r e l b V_{rel}^b Vrelb와 완전히 일치 (코드 출력 [검증] ... 일치: True). 이 "공식 → 수치"의 순환은 연습 1의 표준 답변.
6. 폐쇄 제약과 포획 행렬 G G G: 양손이 물체와 "용접"되어 있는 경우
6.1 제약 방정식
양손이 같은 강체를 잡고 있음 → 종단 속도와 물체 속도 간 일치성이 필요: 물체가 어떻게 움직이는지에 따라 접촉점도 따라 움직여야 한다. 포획 행렬 G G G로 표현:
G [ V L V R ] ⏟ 접촉점 속도 = V o b j (물체의 운동이 접촉점 운동 결정) \underbrace{G\,\begin{bmatrix} V_L \\ V_R \end{bmatrix}}_{\text{접촉점 속도}} = V_{obj}\qquad\text{(물체의 운동이 접촉점 운동 결정)} 접촉점 속도 G[VLVR]=Vobj(물체의 운동이 접촉점 운동 결정)
다른 동등한 표현: 물체가 강체 → 두 접촉점 간 상대 운동은 0 → §4의 V r e l = 0 V_{rel} = 0 Vrel=0 (양손이 물체에 강성 연결된 상태). 폐쇄 제약의 본질 = "상대 속도 고정".
6.2 평면 포획 행렬 G G G 유도 (코드 grasp_matrix 설명)
코드는 n c = 2 n_c=2 nc=2개의 접촉점(양손이 물체를 잡고, 물체는 x = 0 x=0 x=0, 접촉점은 x = + 0.5 x=+0.5 x=+0.5와 x = − 0.5 x=-0.5 x=−0.5)을 사용. 각 접촉점의 힘 f i = ( f x , i , f y , i ) f_i = (f_{x,i}, f_{y,i}) fi=(fx,i,fy,i), 총 2 n c = 4 2n_c = 4 2nc=4 성분. 물체의 합력/력모멘트(3개)는 접촉력의 선형 조합으로 구함:
[ F x F y M ] ⏟ 물체 힘 스크롤 F o b j = [ 1 0 1 0 0 1 0 1 − 0.5 0.5 0.5 − 0.5 ] ⏟ G [ f x , 1 f y , 1 f x , 2 f y , 2 ] ⏟ 접촉력 F c \underbrace{\begin{bmatrix} F_x \\ F_y \\ M \end{bmatrix}}_{\text{물체 힘 스크롤 }F_{obj}} = \underbrace{\begin{bmatrix} 1&0&1&0\\ 0&1&0&1\\ -0.5&0.5&0.5&-0.5 \end{bmatrix}}_{G}\, \underbrace{\begin{bmatrix} f_{x,1}\\ f_{y,1}\\ f_{x,2}\\ f_{y,2} \end{bmatrix}}_{\text{접촉력 }F_c} 물체 힘 스크롤 Fobj FxFyM =G 10−0.5010.5100.501−0.5 접촉력 Fc fx,1fy,1fx,2fy,2
- 1행: F x = f x , 1 + f x , 2 F_x = f_{x,1} + f_{x,2} Fx=fx,1+fx,2 (두 손이 함께 밀어주는 합력);
- 2행: F y = f y , 1 + f y , 2 F_y = f_{y,1} + f_{y,2} Fy=fy,1+fy,2;
- 3행: 힘모멘트 = 각 손의 "힘 × 힘팔" 대수 합.
성실한 설명: 코드는 각 성분의 힘팔을 r = ± 0.5 r=\pm 0.5 r=±0.5로 간단히 표현하여 교육용 행렬이다. 정확한 접촉력 모멘트는 τ = r × f \tau = r\times f τ=r×f (접선 힘에만 힘팔이 있음), 엄격한 버전은 10편 §4 참조. 여기서 핵심은 구조: F o b j = G F c F_{obj} = G\,F_c Fobj=GFc, G G G의 행 수 = 물체 자유도, 열 수 = 접촉력 성분.
6.3 내력 차원: "영공간"에서 세기
n c = 2 n_c=2 nc=2일 때: G G G는 3 × 4 3\times4 3×4, r a n k ( G ) = 3 \mathrm{rank}(G)=3 rank(G)=3이므로 영공간 차원 = 4 − 3 = 1 = 4-3 = 1 =4−3=1. 이 1차원 영공간이 내력: 방향 = 양손 연결선 방향으로 "서로 밀거나 당기는 힘".
3D 상황: 물체 6 자유도, 두 팔 종단 6 접촉력 스크롤 → G G G는 6 × 12 6\times12 6×12, 일반적으로 rank = 6 → 영공간 6차원. 여기서 "접촉은 밀 수만 있고 당길 수 없음 + 마찰錐"으로 4차원이 줄어들어 2차원 = 09편 시작에서 말한 "2개 내력 자유도". 숫자가 일치: 내력 차원 = 포획 행렬의 영공간 차원.
7. 내력 제어: 박스를 잡는 힘
7.1 내력이란?
정의: 물체의 순수 힘/력모멘트를 발생시키지 않는 접촉력 조합. 양손이 서로 밀거나 당기며, 물체는 움직이지 않지만 손과 물체 사이 압력이 큼 —— 이것이 내력.
비유: 손으로 계란을 " firmly " 잡는다. 손은 움직이지 않고, 계란도 움직이지 않음 (물체 순수 힘 = 0), 하지만 잡는 힘이 너무 크면 계란이 깨지고, 너무 작으면 미끄러짐 —— "잡는 힘"이 바로 내력. 박스를 "포함"하거나 "꽉 잡는 것"은 모두 내력 제어.
7.2 전체 접촉력에서 내력을 "분리"하는 방법
전체 접촉력 F c F_c Fc는 두 부분으로 나뉜다 (09 코드 internal_force가 이 방법으로 계산):
F c = G † F o b j ⏟ 물체 운동에 기여하는 성분 + ( I − G † G ) F c ⏟ 내력 ∈ ker ( G ) F_c = \underbrace{G^\dagger\,F_{obj}}_{\text{물체 운동에 기여하는 성분}} + \underbrace{\big(I - G^\dagger G\big)F_c}_{\text{내력}\ \in \ker(G)} Fc=물체 운동에 기여하는 성분 G†Fobj+내력 ∈ker(G) (I−G†G)Fc
- G † G^\dagger G†은 G G G의 의사 역행렬 (01편 참조);
- 검증: G F i n t = G ( I − G † G ) F c = ( G − G G † G ) F c = 0 G\,F_{int} = G(I-G^\dagger G)F_c = (G - GG^\dagger G)F_c = 0 GFint=G(I−G†G)Fc=(G−GG†G)Fc=0 —— 내력은 물체에 대해 0으로 투영, 순수 힘 발생하지 않음.
실제 데이터 (코드 main): 접촉력 F c = ( 10 , 5 , − 10 , 5 ) F_c = (10,5,-10,5) Fc=(10,5,−10,5) (양손 모두 위로 5N 밀기, 동시에 x 방향으로 서로 10/−10N 압력):
F o b j = G F c = ( 0 , 10 , − 10 ) , F i n t = ( 5 , 5 , − 5 , − 5 ) , G F i n t = 0 F_{obj} = G F_c = (0,\ 10,\ -10),\qquad F_{int} = (5,\ 5,\ -5,\ -5),\qquad G F_{int} = 0 Fobj=GFc=(0, 10, −10),Fint=(5, 5, −5, −5),GFint=0
해석: 양손 "합력"은 위로 10N, 반시계 방향 힘모멘트 10 (물체가 받는 유효 하중); 남은 F i n t F_{int} Fint: 각 손은 x 방향으로 각각 ± 5N 압력, 각 손은 5N의 "자기 균형" y 힘 —— 상호 상쇄되어 물체에 대한 순수 효과는 0. 압력 5+5=10N가 "잡는 힘"이며, 이는 §7.3에서 계획해야 할 것이다.
7.3 내력의 이중 모순: 반드시 "계획 + 제어", 무시할 수 없음
| 원하는 것 | 내력 크기 |
|---|---|
| 미끄러짐 방지 (마찰 ∝ \propto ∝ 법선력) | 크게 |
| 압박 파손 방지 / 변형 방지 (물체, 정밀 부품) | 작게 |
따라서 내력은 "계획"된 목표량 (예상 내력 F i n t d e s F_{int}^{des} Fintdes)이며, 결과가 아닌 목표. 양팔 운반 최소 필요한 내력 (하중 F l o a d F_{load} Fload, 마찰 계수 μ \mu μ, 양손 분담):
f n ≥ ∥ F l o a d ∥ 2 μ f_n \ge \frac{\|F_{load}\|}{2\mu} fn≥2μ∥Fload∥
실제 데이터: 10N 박스, 손-박스 마찰 계수 μ = 0.5 \mu = 0.5 μ=0.5: f n ≥ 10 2 × 0.5 = 10 f_n \ge \frac{10}{2\times0.5} = 10 fn≥2×0.510=10 N. 5N 압력은 미끄러짐, 12N 압력은 안정적인 잡음. 이 공식은 "얼마나 잡는가"의 공학적 답.
7.4 내력을 제어 식에 포함
작업 공간 분해 제어 식 (구조는 Caccavale 협력 제어에서 파생, 자세한 내용은 10편):
τ = J T [ Λ ( K p x ~ + K d x ~ ˙ ) ⏟ 물체 위치/자세 작업 + G † T F i n t d e s ⏟ 내력 작업 + F f f ⏟ 전방향(중력/관성 보상) ] \tau = J^T\Big[\underbrace{\Lambda\,(K_p \tilde{x} + K_d \dot{\tilde{x}})}_{\text{물체 위치/자세 작업}} + \underbrace{G^{\dagger T}F_{int}^{des}}_{\text{내력 작업}} + \underbrace{F_{ff}}_{\text{전방향(중력/관성 보상)}}\Big] τ=JT[물체 위치/자세 작업 Λ(Kpx~+Kdx~˙)+내력 작업 G†TFintdes+전방향(중력/관성 보상) Fff]
- 첫 번째 항: 물체 자세 오차 x ~ \tilde x x~ 수렴 (06편 계산 힘 방식 참조);
- 두 번째 항: 예상 내력을 관절로 투영;
- 두 항은 직교 해耦 ( ker ( G ) \ker(G) ker(G)와 range ( G T ) \operatorname{range}(G^T) range(GT) 직교, 01편 4부분 공간 정리) → 물체 운동 작업과 내력 작업은 서로 간섭 없음. 이 때문에 "박스 들면서 손 힘 조절 가능".
8. 양팔 임피던스 제어: 양팔이 "탄성적으로" 박스를 잡는 방법
8.1 단일 팔 임피던스가 부족한 이유
06편의 단일 팔 임피던스는 "사람 ↔ 팔" 상호작용. 양팔 시나리오는 "팔 ↔ 팔 ↔ 물체" 삼방향 상호작용: 위치 오차가 생기면 양팔이 서로 충돌 —— 서로 밀고 당기며 물체에 큰 힘을 가함. 따라서 양팔 임피던스는 단일 팔 임피던스에 내력 항을 추가해야 한다.
8.2 두 가지 구현 방식
① 독립 임피던스 + 내력 (각 손은 06편처럼 임피던스를 적용하고, 내력 목표를 더함):
τ i = J i T [ Λ i ( K d , i x ~ i + B d , i x ~ ˙ i ) ⏟ 단일 팔 임피던스 + F i n t , i d e s ⏟ 내력 추가 ] + C i + g i \tau_i = J_i^T\big[\underbrace{\Lambda_i(K_{d,i}\tilde{x}_i + B_{d,i}\dot{\tilde{x}}_i)}_{\text{단일 팔 임피던스}} + \underbrace{F_{int,i}^{des}}_{\text{내력 추가}}\big] + C_i + g_i τi=JiT[단일 팔 임피던스 Λi(Kd,ixi+Bd,ix˙i)+내력 추가 Fint,ides]+Ci+gi
② 물체 임피던스 (물체 + 양팔을 전체로 모델링, 물체 자세에 가상 스프링/댐퍼 적용, 내력 변화에 댐퍼 적용):
F o b j = Λ o b j ( x ¨ o b j r e f + K p x ~ o b j + K d x ~ ˙ o b j ) , F i n t = K i n t ∫ ( F i n t m e a s − F i n t d e s ) F_{obj} = \Lambda_{obj}\big(\ddot{x}_{obj}^{ref} + K_p \tilde{x}_{obj} + K_d \dot{\tilde{x}}_{obj}\big),\qquad F_{int} = K_{int}\int\big(F_{int}^{meas} - F_{int}^{des}\big) Fobj=Λobj(x¨objref+Kpxobj+Kdx˙obj),Fint=Kint∫(Fintmeas−Fintdes)
- 첫 번째 식: 물체가 "보이지 않는 스프링"에 의해 끌림 (위치/자세 수렴);
- 두 번째 식: 내력 측정값과 목표값의 차이를 적분으로 제거 (08편 운동량 관측기와 유사) —— 이것이 "내력 피드백".
왜 "필수적으로" 내력 항이 필요한가: 내력 항이 없으면 양팔이 각자 자신의 목표 임피던스 → 위치 오차가 서로 충돌 → 물체 파손 또는 미끄러짐. 내력 항이 추가되면 물체 작업과 내력 작업이 해耦되어, 양팔이 "적절한 힘"을 분배. 이는 양팔 알고리즘 엔지니어의 핵심 경쟁력이며, 10편 "협력 조작"의 예비.
9. 협력 계획 (네 가지 핵심)
- 시간 동기화: 양팔 경로는 동일 시간 스케일로 계산 (TOPP, 03편), 핵심 점은 동시에 도달 —— 침대를 들 때 한 손이 0.5초 느리면 침대가 기울어짐;
- 충돌 없는 계획: 양팔 C 공간을 공동 계획 (고차원), 또는 각각 계획 후 충돌 검증 (04편);
- 중복 분배: 폐쇄 체인에서는 중복 제한, 중복은 "내력 유지 + 충돌 회피 + 협력 작업 공간 확장"에 활용 (02편 영공간에 해당, 제약은 "상대 고정");
- 작업 공간 분석: 협력 작업 공간 = 양팔 도달 공간 교집합 (조립 시 양손이 동시에 목표에 도달 가능한지 계산).
10. 코드 전체 해설 (code/09_dual_arm.py)
10.1 adjoint_se2(T) —— §2.3의 행렬, 세 번째 열의 부호 주의
def adjoint_se2(T):
R, p = T[:2, :2], T[:2, 2]
Ad = np.zeros((3, 3))
Ad[:2, :2] = R
Ad[2, 2] = 1.0
Ad[:2, 2] = np.array([p[1], -p[0]]) # 세 번째 열 = (p_y, -p_x), 부호 주의!
return Ad
10.2 relative_jacobian(J_L, J_R, T_L, T_R) —— §5의 블록 공식
def relative_jacobian(J_L, J_R, T_L, T_R):
"""V_rel^b = T_rel^{-1} dT_rel = V_R^b - Ad(T_rel^{-1}) V_L^b
J_rel = [ -Ad(T_rel^{-1}) J_L , J_R ] # 3 x (nL+nR)"""
T_rel = np.linalg.inv(T_L) @ T_R
return np.hstack([-adjoint_se2(np.linalg.inv(T_rel)) @ J_L, J_R])
Ad(T_rel^{-1}) = inv(Ad(T_rel))(Ad의 역복합 성질), 코드는 T r e l − 1 T_{rel}^{-1} Trel−1에 대해 직접 Ad 계산, 물리적으로 직관적.
10.3 grasp_matrix / internal_force —— §6-7의 G와 내력 분해
def grasp_matrix(n_contacts):
"""G (3 x 2n): 물체 합력 = G @ 접촉력; 내력 = ker(G) 성분"""
G = np.zeros((3, 2 * n_contacts))
for i in range(n_contacts):
r = 0.5 if i == 0 else -0.5
G[:2, 2*i:2*i+2] = np.eye(2)
G[2, 2*i:2*i+2] = np.array([-r, r])
return G
def internal_force(G, F_contacts):
"""F_c = G^+ F_obj + (I - G^+ G) F_c ; 내력 반환 = (I - G^+ G) F_c"""
F_obj_comp = G.T @ np.linalg.lstsq(G @ G.T, G @ F_contacts, rcond=None)[0]
return F_contacts - F_obj_comp
10.4 verify_relative_jacobian() —— 본 편에서 가장 중요한 "자기 검증"
수치 미분으로 상대 twist를 독립적으로 계산하고, 공식 계산과 비교 (오차 < 10 − 4 10^{-4} 10−4만 정확). 이는 §4.3/§5.3의 숫자 원천이며, 연습 1의 표준 답변. 이후 모든 twist/제약행렬 공식 수정 시 반드시 이와 같은 자가 검증 추가 —— 이전 문서의 부호 오류는 이 없었기에 발견되지 않았다.
10.5 실행 결과 (python3 code/09_dual_arm.py)
[상대 제약행렬] 크기: (3, 14) (3x14 = 3 x (7+7)여야 함)
[Ad] Ad(T) =
[[ 0.8776 -0.4794 0.5 ]
[ 0.4794 0.8776 -1. ]
[ 0. 0. 1. ]]
[Ad] V_body = [1. 0. 0.3] -> V_spatial = Ad Vb = [1.0276 0.1794 0.3 ]
[Ad] V_spatial = Ad(T) V_body 검증: True
[상대 자세] T_L = ... T_R = ... T_rel = ... (§3.2 참조)
[검증] Vrel_num: [-0.1539 0.3251 0.2 ]
[검증] Vrel_est: [-0.1539 0.3251 0.2 ]
[검증] 상대 제약행렬과 T_rel 체 twist 일치: True
[포획 행렬] G 크기: (3, 4) (3x4)
[포획 행렬] rank(G): 3 -> ker(G^T) 차원: 1
[내력] 접촉력 ker(G^T) 투영: [ 5. 5. -5. -5.]
[내력] 검증: G^T(내력)=0 -> [0. 0. 0.]
11. 당신의 기술 스택에 적용 (MoveIt2 / ros2_control / 양팔)
| 본 편 개념 | 구현 방법 |
|---|---|
| 상대 자세 T r e l = T L − 1 T R T_{rel} = T_L^{-1}T_R Trel=TL−1TR | 두 종단 TF: tf2에서 직접 T_L.inverse() * T_R 계산, 또는 두 종단 frame 리스닝 후 변환 계산 |
| 상대 제약행렬 J r e l J_{rel} Jrel | MoveIt2의 Jacobian 각 팔 종단별 계산 (body form), §5에 따라 블록 조합; 또는 양팔 그룹 연계 getJacobian |
| 폐쇄 제약 | MoveIt2에서 두 팔을 하나의 planning_group으로 (양팔 SRDF), 연계 계획으로 제약 자동 적용 |
| 내력 제어 | F/T 센서 필요 + 사용자 정의 양팔 컨트롤러 (ros2_control 플러그인 레벨), §7.4의 τ \tau τ를 두 팔 joint groups에 분배 |
| 양팔 임피던스 | 두 servo_node 인스턴스 (다른 ns), 각각의 도우미, 내력 목표 명령 추가 (06/08편 참조) |
| 실시간 서보 | fr5_servo_teleop 각 팔별 인스턴스, 마스터-슬레이브 방식: 마스터 팔 경로, 슬레이브 팔 도우미 따라움직임 |
당신의 양팔 7축 구현 경로 (복잡도 및 가치 순으로): ① 마스터-슬레이브 운반 (왼팔 경로 + 오른팔 도우미 따라움직임) —— 가장 빠르게 효과; ② 상대 자세 유지 (J r e l J_{rel} Jrel로 V r e l → 0 V_{rel}\to 0 Vrel→0 클로즈드 루프); ③ 대칭 + 내력 피드백 (F/T 센서 + §8.2 물체 임피던스) —— 이는 면접에서 "가산점".
12. 핵심 문헌 (취업 면접/연구 필독)
- Chiaverini 등: 중복 로봇 + 작업 우선순위 (중복 요약, §5.2 영공간에 해당)
- Khatib: Operational space control (작업 공간 제어, §8 물체 임피던스 기반)
- Caccavale: Cooperative manipulators (양팔 내력 제어 요약, §7/§10 제어식 출처)
- Uchiyama & Dauchez: Symmetric kinematic formulation (대칭 양팔 운동학, 본편 상대 twist/제약행렬 출처)
- Itoh: 협력 양팔 + 내력 제어 구현 (산업 사례)
연습 (정답은 본 편 §10 코드에 있음)
- 상대 제약행렬 검증:
python3 code/09_dual_arm.py실행,verify_relative_jacobian의 임의 속도를 ( q ˙ L , q ˙ R ) (\dot q_L,\dot q_R) (q˙L,q˙R)로 변경하여 J r e l J_{rel} Jrel 수치 미분 자가 검증이 항상True인지 확인. - 오류 공식 변경해 보기:
relative_jacobian를 이전Ad(T_rel) J_R - J_L로 변경하고 자가 검증 실행 —— 오차가 얼마나 커지는지 확인하고, 왜 블록 형식이 올바른지 이해. - 내력 계산:
grasp_matrix(2)와internal_force로 임의 접촉력을 계산하여 G F i n t = 0 G\,F_{int}=0 GFint=0 검증하고, "어떤 부분이 물건 들기 힘, 어떤 부분이 잡는 힘인지" 설명. - 최소 내력: 박스 20N, μ = 0.4 \mu=0.4 μ=0.4, 양손으로 들면 최소 법선 잡는 힘 계산, 마찰錐 지식으로 미끄러지는지 판단.
- 생각해 보기: 양팔 7축이 단일 팔보다 중복 자유도를 어떻게 "내력 유지 + 충돌 회피 + 작업 공간 확장"에 배분하는가? (02편 영공간 투영을 참고하여 의사코드 작성)
단계별 요약 · 양팔 협력 프로젝트 제안
01-09를 "양팔 조립" 졸업 프로젝트로 연결:
시각 인식 → 손-눈 보정(01/02) → 양팔 상대 자세 정렬(09) →
양팔 협력 경로(03/09) → 내력 피드백 임피던스(06/09) → 구멍과 축 맞춤 탐색 → 완료
- 전 8편은 "두 다리", 09는 "두 다리를 같은 침대에 연결" —— 상대 자세 = 인터페이스, 상대 제약행렬 = 전달, 내력 = 손잡기.
- 10편은 "손잡기"를 더 깊이 있게 설명 (력 폐쇄, 마찰錐, Caccavale 협력 제어) —— 09는 운동학 골격, 10은 역학 근육.
학습 잘 되었기를 바랍니다, 궁금한 점이 있으면 언제든지 질문하세요.
이전 글: 08_상태 추정과 양측 원격 조작.md
다음 글: 10_양팔 조작과 협력 제어.md