15장. 팔 전체를 제어하기: 계산 토크 제어와 작업 공간 제어

관절마다 따로 PD를 걸면 관절끼리의 얽힘이 외란으로 남는다. 이 장은 PD + 중력 보상이 왜 안정한지 에너지로 보이고, 모델로 얽힘을 지우는 계산 토크 제어를 만든다. 그다음 같은 구조를 손끝으로 옮겨, 손끝을 직접 끌어당기는 작업 공간 제어를 완성한다. 강의 15강(72분)에 해당한다.

15.1 관절이 둘이면: 얽힘이 외란이 된다

강의는 1993년에 만든 영상으로 시작한다. 다리로 뛰어다니는 Raibert의 달리는 기계다. 교수는 시뮬레이션이 늘 들어맞지는 않으니 실제 로봇으로 실험해 확인하는 것이 중요하다고 덧붙인다.

13~14장에서 가져올 것

13~14장은 질량 하나(자유도 1)를 제어했다. 이 장에서 쓰는 것만 다시 적는다.

• 질량 m에 PD 제어 F = −k_p(x − x_d) − k_v ẋ를 걸면, 용수철과 댐퍼를 단 것처럼 움직인다(13.9절). 에너지가 줄기만 하니 안정하다(13.8절).
• 제어 분할 F = αF′ + β에서 α = m̂(질량 추정), β는 모델로 아는 나머지 힘이다. 그러면 ẍ = F′인 단위 질량 시스템이 남는다(14.4절).
• 궤적을 따라가려면 F′ = ẍ_d − k_v′ė − k_p′e로 한다(e = x − x_d). k_p′ = ω_n², k_v′ = 2ω_n이면 임계 감쇠다(14.3, 14.5절).
• 외란 F_d가 있으면 정상 상태 오차 F_d / (m k_p′)가 남는다(14.6절). 이득이 클수록 작다.

운동 방정식을 두 줄로

종이 팔의 M q̈ + V + G = τ(12장)를 관절마다 한 줄씩 풀어 쓴다.

관절 1의 눈으로 보자. 자기 관성 × 자기 가속도인 M₁₁θ̈₁ 말고도 세 가지가 더 걸린다. 팔꿈치가 가속하며 끄는 몫 M₁₂θ̈₂, 원심력·코리올리 힘 V₁, 중력 G₁이다.

가장 쉬운 방법은 관절마다 질량 하나로 보고, 나머지 세 가지는 외란(disturbance)으로 치는 것이다. 14.9절의 독립 관절 제어(independent joint control)다. 강의는 오늘날 공장 로봇도 대부분 이렇게 한다고 말한다.

외란이 크다

문제는 이 외란이 작지 않다는 것이다. 한 관절이 가속하면 다른 관절이 곧바로 끌려가고, 빨리 움직일수록 원심력·코리올리 힘도 커진다. 14.6절의 공식대로 오차를 줄이려면 이득을 아주 크게 올려야 한다.

14.9절에서 본 대로 기어비가 큰 팔은 관절마다 관성이 거의 일정하고 얽힘이 상대적으로 작아서, 독립 관절 제어가 잘 통한다. 기어가 작거나 없는 가볍고 빠른 팔일수록 얽힘이 크게 드러난다.

이 장은 두 가지를 묻는다. 얽힘이 있는데도 PD만으로 안정한가(15.2절), 그리고 이득을 마음껏 올릴 수 없다면 어떻게 할까(15.3~15.4절)다. 그 답을 손끝에 옮긴 것이 작업 공간 제어다(15.5~15.9절).

영상에서 보기: 01:19 1993년 영상 · 02:54 실험으로 확인한다 · 04:48 두 줄로 쓴 운동 방정식 · 07:28 독립 관절 제어

15.2 PD + 중력 보상은 왜 안정한가

관절이 얽혀 있고 M도 자세마다 바뀌면, 식을 하나씩 풀어서는 안정한지 알기 어렵다. 강의에 따르면 1970년대 말에 이 문제를 다룬 논문이 많이 나왔다. 답은 13.8절처럼 식을 풀지 않고 에너지 하나만 보는 데서 나온다.

제어 법칙

관절 모두에 한꺼번에 거는 PD에 중력 추정 Ĝ를 더한다. K_p와 K_v는 관절마다 이득을 대각에 놓은 행렬이다.

수학 더하기: 위치 에너지의 기울기. 위치 에너지 U(q)가 있으면 그 힘은 −dU/dq다. 내리막 쪽으로 민다. 용수철 U = ½k(q − q_d)²이면 힘은 −k(q − q_d)다.

변수가 여럿이면 변수마다 편미분해 늘어놓은 ∂U/∂q를 쓴다. 이것을 기울기(gradient)라 한다. 12.9절의 G = ∂U/∂q가 바로 중력 에너지의 기울기였다.

그러니 −K_p(q − q_d)는 목표 q_d에 바닥이 있는 가상의 용수철이 미는 힘이다. 13.9절의 그릇 U_goal을 관절 여러 개로 넓힌 U_goal = ½(q − q_d)ᵀK_p(q − q_d)의 기울기다.

한편 +Ĝ는 중력을 받쳐 준다. Ĝ = G이면 중력의 위치 에너지가 지워진다. 결국 이 제어기는 팔이 느끼는 위치 에너지를 중력 대신 목표에 바닥이 있는 그릇으로 바꿔 끼운다.

해 볼 것 네 가지:

1. 중력 보상 0%, 목표 0°, K_p = 100: 바닥이 −16.7°에 있다. 팔이 목표보다 처진다.
2. K_p를 300으로 올린다. 그릇이 좁고 깊어져 처짐이 −5.8°로 준다. 이득을 올려도 처짐이 0이 되지는 않는다.
3. 중력 보상 100%: 이득과 상관없이 바닥이 정확히 목표에 온다. 50%면 그 사이다.
4. 중력 보상 0%로 목표를 90°(똑바로 세움)에 둔다. 처짐이 0이다. 그런데 K_p를 20으로 내리면 바닥이 3.1°와 176.9° 두 곳으로 갈라진다.

잠깐, 생각해 보기. 4번에서 똑바로 세운 팔은 왜 중력 보상 없이도 처지지 않을까? K_p가 작으면 왜 바닥이 갈라질까?

곧게 선 팔에는 중력 토크가 0이라 용수철이 버틸 것이 없다(12.9절). 하지만 그 자리는 중력만 보면 언덕 꼭대기다.

K_p가 30.4 mN·m/rad(중력 토크의 최댓값과 같은 숫자)보다 크면 용수철 그릇이 언덕을 이겨 바닥이 된다. 작으면 못 이겨서 팔이 한쪽으로 쓰러진다.

에너지가 줄기만 한다

이제 관절이 여럿이고 얽혀 있어도 안정하다는 것을 보인다. Ĝ = G라 하자. 운동 에너지와 용수철 에너지를 더한 E를 본다.

E를 시간으로 미분할 때 쓸 사실은 하나다. 운동 에너지가 변하는 빠르기는 q̇ᵀ(M q̈ + V)다. 원심력·코리올리 힘은 관절끼리 에너지를 주고받게 할 뿐, 팔 전체의 에너지를 늘리거나 줄이지 않는다는 뜻이다.

제어된 팔은 M q̈ + V = −K_p(q − q_d) − K_v q̇를 따른다. 이것을 넣으면 용수철 몫이 서로 지워진다.

읽는 법은 이렇다.

1. K_v = 0이면 E가 그대로다. 팔은 그릇 안에서 영원히 출렁인다. 안정하지만 멈추지 않는다.
2. K_v가 양수이면 팔이 움직이는 동안(q̇ ≠ 0) E가 계속 준다. 감쇠 항은 늘 속도와 반대로 밀어서 에너지를 빼낸다.
3. E가 줄기를 멈추는 때는 팔이 멈출 때뿐이다. 그런데 q ≠ q_d에서 멈추면 용수철이 다시 밀어 움직이게 한다. 그러니 팔은 결국 q = q_d에 멈추고, 이것을 점근 안정(asymptotically stable)이라 한다.

잠깐, 생각해 보기. 이 증명에는 M의 값도, 얽힘의 크기도 들어가지 않았다. 무엇만 썼나?

세 가지뿐이다. M이 양의 정부호라 운동 에너지가 0 이상이고(12.5절), V가 에너지를 넣지 않으며, K_p와 K_v가 양수다. 그래서 어떤 팔에서나 성립한다.

대신 가는 동안 얼마나 흔들리고 얼마나 길을 벗어나는지(성능)는 말해 주지 않는다.

목표 자세로 가기만 하면 되는 일에는 PD + 중력 보상으로 충분하다. 하지만 궤적을 따라가야 하면 얽힘이 그대로 외란이 된다. 그것을 누르는 손잡이는 K_p와 K_v뿐이다.

영상에서 보기: 08:31 PD 제어는 안정한가 · 10:36 비례 항은 위치 에너지의 기울기 · 12:11 감쇠 항이 점근 안정을 준다 · 13:41 중력 보상

15.3 이득은 얼마나 올릴 수 있나

외란을 잘 누르려면 K_p를 키우고, 감쇠를 맞추려면 K_v도 같이 키운다. 그런데 14.3절에서 본 대로 이득은 끝없이 올릴 수 없다. 이 강의는 한계를 세 가지로 정리한다.

• 구조의 유연성: 링크와 구동부는 조금씩 휜다. PUMA는 모터가 링크 안쪽에 있고 긴 막대로 관절을 돌려서 그 막대가 휜다. 닫힌 루프의 ω_n이 구조의 공진 진동수에 가까워지면 그 떨림을 키운다.
• 시간 지연: 엔코더를 읽고, 계산하고, 모터가 토크를 내기까지 시간이 걸린다.
• 샘플링 주기: 제어 루프가 1초에 몇 번 도는가다. 요즘 컴퓨터는 빨라서 거의 문제가 되지 않는다.

유인물의 어림 규칙은 이렇다. 첫 줄은 14.3절의 규칙과 같다.

예를 들어 가장 낮은 공진이 8 Hz, 지연이 10 ms, 서보 루프가 1 kHz라 하자. 차례로 ω_n ≤ 25, 209, 1257 rad/s다. 가장 작은 25 rad/s가 한계이고, 거의 늘 구조의 유연성이 한계를 정한다.

ω_n = 25 rad/s를 종이 팔 어깨(M̄ = 2487 g·cm²)의 이득으로 바꾸면 K_p = M̄ω_n² ≈ 157 mN·m/rad다. 이 이상은 못 쓴다. 그러니 이득으로 못 누르는 오차는 다음 절처럼 모델로 줄여야 한다.

영상에서 보기: 14:43 높은 이득이 필요하다 · 15:14 구조의 유연성 · 16:19 시간 지연 · 17:24 이득의 어림 규칙

15.4 모델로 얽힘을 지운다: 관절 공간의 비선형 동적 분리

이득만으로 외란을 다 누를 수 없다면, 외란을 미리 계산해 빼 주면 된다. 11~12장에서 M, V, G의 모양을 구하는 법을 배웠다. 그 안의 질량, 질량 중심 위치, 관성은 재거나 실험으로 찾는다.

14장의 αF′ + β를 행렬로

14.4절에서는 질량 하나에 F = m̂F′ + β를 걸어 단위 질량을 만들었다. 이번에는 α 자리에 질량 행렬 추정 M̂(q)를, β 자리에 V̂ + Ĝ를 넣는다.

이 τ를 운동 방정식 M q̈ + V + G = τ에 넣고 양쪽에 M⁻¹을 곱한다.

추정이 완벽하면(M̂ = M, V̂ = V, Ĝ = G) 첫 항은 τ′, 둘째 항은 0이다. 남는 것은 q̈ = τ′ 하나다. 교수는 이것을 "마법 같다"고 한다.

관절마다 가속도가 자기 τ′로만 정해진다. 얽힘도, 자세에 따라 바뀌는 관성도 사라지고 관절마다 단위 질량이 하나씩 남는다. 이것을 비선형 동적 분리(nonlinear dynamic decoupling)라 하고, 계산 토크 제어(computed torque control)라고도 한다.

τ′는 단위 질량에 거는 제어

이제 τ′에는 단위 질량에 맞춰 설계한 아무 제어나 넣을 수 있다. 궤적 q_d(t)를 따라가려면 14장의 꼴을 관절마다 쓴다.

관절마다 따로 떨어진 질량–용수철–댐퍼다. k_p′ = ω_n², k_v′ = 2ω_n으로 고르면 모든 자세에서 임계 감쇠다. 이득을 M̂가 자세에 맞춰 알아서 키우고 줄여 주기 때문이다.

직접 비교하기

아래 그림은 책상 위의 종이 팔이 1초 동안 (−60°, 120°)에서 (60°, 30°)로 가는 궤적을 따라간다. 두 제어기는 같은 τ′(앞먹임 q̈_d 포함, 같은 ω_n)를 쓴다. 다른 것은 τ′에 곱하는 관성뿐이다.

• 독립 관절 제어: 관절마다 상수 관성 M̄ = (2487, 320) g·cm²를 곱한다. 얽힘과 V는 외란으로 남는다.
• 계산 토크: M̂(q)를 곱하고 V̂를 더한다.

해 볼 것 세 가지:

1. ω_n = 10, 물건 0 g: 독립 관절 제어는 오차가 12.1°까지 커진다. 계산 토크는 0.0°다.
2. ω_n을 30으로 올린다. 독립 관절 제어도 1.9°로 준다. 하지만 15.3절의 한계 때문에 늘 이렇게 올릴 수는 없다.
3. ω_n = 10에서 물건을 2 g, 5 g으로 늘린다. 계산 토크에도 오차(3.7°, 9.6°)가 생기지만 독립 관절 제어(14.7°, 18.1°)보다 작다.

모델이 틀리면

추정은 완벽하지 않다. 위 식의 둘째 항이 작은 외란 ε로 남고, 그것은 k_p′와 k_v′가 누른다. 그래도 처음의 얽힘보다 훨씬 작아서 훨씬 작은 이득으로 된다.

무엇이 틀렸는지에 따라 결과가 다르다.

• Ĝ가 틀리면: 일정한 토크가 남아 정상 상태 오차 e = (M k_p′)⁻¹(Ĝ − G)가 생긴다. 14.6절처럼 외란을 (질량 × 이득)으로 나눈 꼴이다. 안정성은 해치지 않는다.
• M̂가 틀리면: M⁻¹M̂가 단위행렬이 아니라서 이득을 조금 키우거나 줄인 것과 같다. M̂가 양의 정부호이기만 하면 크게 위험하지 않다.
• V̂가 틀리면: 위험할 수 있다. V는 속도에 달린 힘이라, 틀린 V̂는 속도와 같은 쪽으로 미는 힘(감쇠를 깎는 힘)이 될 수 있다.

잠깐, 생각해 보기. 강의는 V̂를 잘 모르겠으면 틀리게 넣느니 차라리 V̂ = 0으로 두라고 한다. 왜 그럴까?

V̂ = 0이면 남는 것은 팔이 원래 가진 원심력·코리올리 힘이다. 15.2절에서 본 대로 이 힘은 팔에 에너지를 새로 넣지 않는다. 틀린 V̂는 속도에 비례해 에너지를 넣는 힘이 되어 팔을 흔들 수 있다.

알아 두기. M q̈ + V + G = τ에는 마찰도, 링크의 휨도, 모터의 빠른 전기적 동역학도 없다. 특히 모터와 감속기의 마찰은 속도에 따라 들쭉날쭉해서 모델로 다 지우기 어렵다. 그래서 모델을 써도 어느 정도의 이득은 여전히 필요하다.

영상에서 보기: 19:28 모델을 쓰자 · 21:33 τ = M̂τ′ + V̂ + Ĝ · 24:41 비선형 동적 분리 · 31:32 V̂를 모르면 0으로

15.5 손끝을 끌어당긴다: 작업 공간 제어의 생각

15.4절의 제어기는 관절의 궤적 q_d(t)가 있어야 돈다. 그런데 일은 손끝으로 주어진다. 카메라가 컵을 보고 "손을 저기로"라고 하면, 길 위의 점마다 역기구학을 풀어 q_d로 바꿔야 한다.

여유가 있으면 더 어렵다

강의 예는 바퀴 달린 받침 위에 팔을 얹은 이동 로봇이다. 손끝을 물건에 대는 자세는 끝없이 많다(2.3절의 여유자유도). 어느 자세로 갈지 미리 정하기 어렵고, 정해 버리면 잃는 것도 있다.

받침은 무겁고 팔은 가볍다. 가벼운 팔로 먼저 재빨리 뻗고 무거운 받침은 천천히 따라오게 하면 손이 훨씬 빨리 닿는다. 자세를 미리 정해 관절 궤적으로 만들면 이런 성질을 쓰기 어렵다.

여유자유도 제어는 다음 학기 고급 로봇공학에서 다룬다.

손끝을 당긴다

1.5절의 "자석"이 답이다. 교수가 오래전에 낸 생각으로, 팔이 어떻게 움직일지 미리 정하지 않고 손끝만 당긴다. 목표에 바닥이 있는 위치 에너지를 손끝 좌표 x로 만든다.

손끝은 이 그릇 안으로 굴러떨어진다. 손끝에 힘 F를 거는 토크는 8.8절에서 이미 안다.

이렇게 손끝 좌표로 일을 적고 제어하는 것을 작업 공간 제어(operational space control) 또는 작업 지향 제어(task-oriented control)라 한다. 관절 각도를 미리 계산하지 않으니 역기구학이 필요 없다.

J를 cm로 적었으니 JᵀF는 mN·cm로 나온다. 100으로 나누면 mN·m다.

해 볼 것 세 가지:

1. 처음 값(목표 (−4, 6) cm): F = (−25.3, −14.0) mN이고 τ = (2.08, 2.03) mN·m다.
2. 목표를 손끝 (8.7, 13.0) 가까이 옮긴다. F와 τ가 함께 0으로 준다. 용수철처럼 거리에 비례한다.
3. 주황 점선을 본다. 처음 가속은 F와 20° 어긋난다. 왜 그런지는 15.6절에서 본다.

잠깐, 생각해 보기. 목표가 닿는 곳 밖(어깨에서 18 cm보다 먼 곳)에 있으면 팔은 어떻게 될까?

손끝이 목표 쪽으로 끌려가다 팔이 쭉 펴진다. 그 자세에서 F는 팔 방향을 향해서, 8.9절처럼 토크를 만들지 못한다. 팔은 목표 쪽으로 뻗은 채 멈추고, 숫자가 터지지도 않는다.

장애물과 접촉도 힘으로

같은 생각으로 장애물에서는 밀어내는 위치 에너지를 링크마다 만든다. 장애물이 움직이면 그 힘이 바로 바뀐다. 관절 공간에서라면 길을 처음부터 다시 계획해야 할 일이다.

물건을 책상에 대고 누르며 미는 일도 쉽다. 누르는 힘 F_contact도 손끝의 힘이라 τ = Jᵀ(F_motion + F_contact)로 더하면 된다. 움직임과 힘을 한 틀에서 제어하는 이 생각은 16강에서 이어진다.

다만 아직 빠진 것이 있다. 이 제어기는 팔의 동역학을 쓰지 않았다. 손끝을 당기면 팔 전체가 끌려가기만 할 뿐, 손끝이 어떤 길로 갈지는 보장하지 않는다.

영상에서 보기: 33:08 관절 궤적은 어디서 오나 · 36:45 손끝을 당긴다 · 38:54 장애물은 밀어낸다 · 41:31 τ = JᵀF

15.6 손끝이 느끼는 질량: 작업 공간의 운동 방정식

질점에 x 방향으로 힘을 주면 x 방향으로 가속한다. 팔의 손끝은 링크가 얽힌 몸이라 엉뚱한 방향으로도 가속한다(그림 15-4의 주황 점선). 손끝에서 힘과 가속도를 잇는 식이 필요하다.

손끝 좌표로 쓴 운동 방정식

제어할 기준점을 손끝의 한 점으로 정하고, 이 점을 작업 점(operational point)이라 한다. 여유가 없는 팔(관절 수 = 작업 좌표 수)이면 작업 점의 좌표 x를 일반화 좌표로 쓸 수 있다.

6관절 팔이면 x = (x, y, z, α, β, γ), 종이 팔이면 x = (x, y)다. 라그랑주 방정식을 q 대신 x로 쓰면 같은 모양의 식이 나온다.

• Λ(x): 손끝의 질량 행렬. 작업 공간의 운동 에너지 행렬이라고도 한다.
• μ(x, ẋ): 손끝에서 본 원심력·코리올리 힘
• p(x): 손끝에서 본 중력
• F: 손끝에 거는 힘(3차원이면 힘과 모멘트)

Λ는 에너지로 찾는다

같은 팔의 운동 에너지를 관절 속도로도, 손끝 속도로도 쓸 수 있다. 둘은 같은 값이다.

ẋ = J q̇를 왼쪽에 넣으면 q̇ᵀ(JᵀΛJ)q̇ = q̇ᵀMq̇다. 어떤 q̇에서나 맞으니 M = JᵀΛJ이고, 양쪽에 J의 역행렬을 곱해 Λ를 꺼낸다.

수학 더하기: J⁻ᵀ와 곱의 역행렬. J⁻ᵀ는 (J⁻¹)ᵀ를 줄여 쓴 것으로, (Jᵀ)⁻¹과 같다. 곱의 역행렬은 순서를 뒤집어 하나씩 역행렬을 취한다. (ABC)⁻¹ = C⁻¹B⁻¹A⁻¹이다.

그래서 (JM⁻¹Jᵀ)⁻¹ = (Jᵀ)⁻¹ M J⁻¹ = J⁻ᵀMJ⁻¹이다. 위의 두 꼴은 같은 행렬이다.

둘째 꼴의 가운데 JM⁻¹Jᵀ에는 뜻이 있다. 멈춘 팔에 손끝 힘 F를 걸면 q̈ = M⁻¹JᵀF이고, 손끝 가속도는 ẍ = Jq̈ = JM⁻¹Jᵀ F다. 곧 "힘을 주면 얼마나 가속하나"이고, 그 역행렬이 질량이다.

알아 두기. 관절이 작업 좌표보다 많은 팔(여유자유도, 2.3절)은 J가 정사각이 아니라 J⁻¹이 없다. 그래도 JM⁻¹Jᵀ는 정사각이라 Λ = (JM⁻¹Jᵀ)⁻¹은 구할 수 있다. 실제 계산에서 이 꼴을 쓰는 까닭이고, 여유자유도 팔의 작업 공간 제어는 고급 로봇공학에서 다룬다.

μ와 p

ẋ = J q̇를 한 번 더 미분하면 ẍ = J q̈ + h다. h는 J가 자세 따라 바뀌어서 생기는 몫으로, 관절을 일정한 빠르기로 돌릴 때(q̈ = 0)의 손끝 가속도다. 손끝이 원을 그리며 안쪽으로 가속하는 몫이다.

q̈ = J⁻¹(ẍ − h)를 관절 공간의 식 M q̈ + V + G = JᵀF에 넣고, 양쪽 앞에 J⁻ᵀ를 곱한다.

h의 몫이 있어서 μ는 V를 옮긴 J⁻ᵀV와 다르다. 관절 공간에서 V를 지워도 손끝의 원심력·코리올리 힘이 다 지워지지는 않는다(강의).

p = J⁻ᵀG는 τ = JᵀF를 거꾸로 쓴 것이다. 관절이 중력을 받치는 토크 G를, 손끝 하나로 받칠 때 드는 힘으로 바꾼다.

종이 팔로 계산하기: θ = (0°, 90°)

링크 1이 x축을 따라 놓이고 링크 2가 위(+y)를 보는 자세다. 6장대로 J를 적고 역행렬을 구한다. det J = l₁l₂ sin θ₂ = 80이다.

12.4절에서 θ₂ = 90°이면 M = [[2486.7, 320], [320, 320]] g·cm²였다. 먼저 MJ⁻¹을 구하고 앞에 J⁻ᵀ를 곱한다.

손끝을 x로 밀면 5 g, y로 밀면 21.7 g으로 느껴진다. 숫자 둘 다 몸으로 확인할 수 있다.

• x 방향 5 g: 팔꿈치는 링크 1에 수직(y)으로만 움직인다. 그러니 손끝을 x로 밀면 링크 2만 팔꿈치를 축으로 돈다. 끝을 축으로 도는 막대의 관성 m₂l₂²/3 = 320을 손끝 거리의 제곱 64로 나누면 5 g이다.
• y 방향 21.7 g: 링크 2는 돌지 않고 통째로 y로 미끄러진다(15 g). 링크 1은 어깨를 축으로 돈다. 그 몫은 (m₁l₁²/3)/l₁² = 6.7 g이고, 합이 21.7 g이다.

미는 방향으로 느끼는 질량

이 자세의 Λ는 대각이라, x로 밀면 x로만, y로 밀면 y로만 간다. 45°로 밀면 어떨까?

멈춘 팔에 방향 u(길이 1)로 힘 F를 주면 ẍ = Λ⁻¹u F이고, 그중 u 방향 몫은 uᵀΛ⁻¹u F다. 그래서 미는 쪽에서 느끼는 질량, 곧 u 방향의 실효 질량(effective mass)은 아래와 같다.

45°이면 uᵀΛ⁻¹u = ½(1/5 + 1/21.7) = 0.123이라 m_u = 8.1 g이다. 두 질량의 평균(13.3 g)보다 가벼운 쪽에 가깝다. 가속도는 가벼운 x 쪽으로 쏠려 미는 방향과 32° 어긋난다.

방향마다 반지름이 √m_u가 되게 그리면 타원이 된다. 긴 축은 가장 무거운 방향, 짧은 축은 가장 가벼운 방향이다. 7.10절의 속도 타원, 8.9절의 힘 타원과 같은 방법으로 그린 질량 타원이다.

해 볼 것 네 가지:

1. θ₂ = 90°에서 미는 방향을 0°나 90°로 둔다. 가속이 미는 방향 그대로이고, 질량은 5.0 g과 21.7 g이다.
2. 미는 방향 45°: 느끼는 질량 8.1 g, 가속은 32° 어긋난다.
3. θ₂를 3° 쪽으로 내려 팔을 편다. 팔 방향 질량이 4270 g까지 치솟는다. 177°까지 접어도 같다.
4. 가장 가벼운 방향은 어느 자세에서나 4.5~5 g쯤이다. 손끝을 옆으로 미는 일은 늘 가볍다.

잠깐, 생각해 보기. 오른쪽 그래프에는 θ₁이 없다. 어깨를 돌리면 타원은 어떻게 될까?

팔 모양이 그대로이니 타원도 모양 그대로 팔과 함께 돈다. 12장의 M에 θ₁이 없었던 것과 같은 까닭이다. Λ의 칸 숫자는 바뀌지만, 가장 무겁고 가벼운 질량은 θ₂만으로 정해진다.

특이점에서 무한대

팔을 쭉 펴면(θ₂ → 0°) 손끝은 팔 방향으로 움직일 수 없다(7.9절의 특이점). 그 방향으로는 아무리 세게 밀어도 가속하지 않으니, 느끼는 질량이 무한대다. 종이 팔은 θ₂가 작을 때 가장 무거운 방향의 질량이 대략 12 g ÷ sin²θ₂다.

강의도 같은 말을 한다. 특이점에 가까이 가면 손끝 질량이 너무 커져서 더 움직일 수 없다. 8.9절의 "팔을 편 쪽으로는 토크 없이 큰 힘을 버틴다"와 같은 방향이다.

영상에서 보기: 44:36 방향마다 다른 질량 · 49:16 에너지로 관계 찾기 · 51:24 Λ = J⁻ᵀMJ⁻¹ · 52:28 μ와 p

15.7 강의 예제: 돌면서 늘어나는 팔의 Λ

강의는 12.10절의 회전 + 직동 팔(RP 팔)로 Λ를 구한다. 손끝 대신 링크 2의 질량 중심(어깨에서 d₂)을 본다. 이 팔은 관절 공간의 M이 대각이었다.

{1}에서 보기

링크를 따라 x₁, 그에 수직으로 y₁을 잡은 좌표계 {1}에서 그 점의 속도는 (ḋ₂, d₂θ̇₁)이다. 늘이면 x₁로, 돌리면 y₁로 간다.

두 칸 모두 뜻이 분명하다.

• x₁ 방향 m₂: 링크를 따라 밀면 링크 2만 미끄러진다. 링크 1은 꼼짝하지 않는다.
• y₁ 방향 m₂ + m₂′: 옆으로 밀면 팔 전체가 돌아야 한다. 링크 1의 질량과 두 링크의 회전 관성이 거리 d₂의 제곱으로 나뉘어 질량 m₂′처럼 보탠다.

{0}에서 보기

받침 좌표계 {0}의 숫자로 바꾸면 Λ가 더는 대각이 아니다. 힘과 가속도는 둘 다 화살표라, {1}의 숫자에 회전행렬 R을 곱하면 {0}의 숫자가 된다(2장). ⁰F = R ¹F, ⁰ẍ = R ¹ẍ를 ¹F = ¹Λ ¹ẍ에 넣으면 ⁰Λ = R ¹Λ Rᵀ다.

비대각 칸 −m₂′s₁c₁이 얽힘이다. x₀로 밀어도 y₀로도 가속한다. 관절 공간에서는 얽힘이 없던 팔도, 일하는 공간에서는 얽혀 보인다는 것이 강의의 요점이다.

θ₁ = 0°이면 s₁ = 0, c₁ = 1이라 ⁰Λ = diag(m₂, m₂ + m₂′)로 다시 대각이다. 링크가 x₀ 축 위에 있어 {0}과 {1}의 방향이 같기 때문이다.

영상에서 보기: 53:32 강의 예제 · 54:33 {1}에서는 대각 · 57:12 {0}으로 돌리면 얽힌다 · 58:17 질량 타원

15.8 손끝의 PD와 비선형 동적 분리

이제 15.2절과 15.4절을 손끝에서 그대로 되풀이한다. 식의 q가 x로, M이 Λ로 바뀔 뿐이다.

모델 없이: 손끝의 PD + 중력 보상

15.5절의 자석에 감쇠와 중력 보상을 더한다.

15.2절과 똑같이 에너지로 안정성을 보일 수 있다. 중력 대신 목표에 바닥이 있는 그릇이 생기고, −k_vẋ가 에너지를 빼낸다. 그래서 손끝은 결국 x_d에 멈춘다(점근 안정).

하지만 가는 길은 보장하지 않는다. p̂ = p일 때 닫힌 루프의 첫째 줄(x 방향)을 쓰면 이렇다.

세 가지가 걸린다. x 방향 질량 Λ₁₁이 자세마다 바뀌고, y 방향 가속이 Λ₁₂ÿ로 끼어들고, μ₁이 외란이 된다. 특이점 가까이서는 Λ₁₁이 무한대로 커져 그 방향으로 움직이지 못한다.

모델로 지우기

15.4절과 같은 구조를 손끝에 쓴다. F′는 손끝을 단위 질량으로 보고 설계한 제어다.

추정이 완벽하면 작업 공간의 식 Λẍ + μ + p = F에서 μ와 p가 지워지고 Λ가 나뉜다. 남는 것은 ẍ = F′ 하나다. 손끝이 방향마다 같은 질량 1을 가진 점처럼 움직인다.

마지막으로 F를 τ = JᵀF로 모터에 보낸다.

x를 q로 바꾸면 15.4절의 식과 같다. 강의가 강조하듯 같은 구조 하나로 관절 공간도, 손끝도 제어한다. Λ̂ = I(단위행렬), μ̂ = 0으로 두면 앞의 "모델 없는 PD"로 돌아간다.

해 볼 것 세 가지:

1. 처음 값(목표 (−6, 12) cm): 파란 길은 곧은 선이다. 주황은 1.67 cm, 회색은 3.29 cm 벗어난다.
2. 목표를 받침 반대쪽 낮은 곳 (−14, 2)로 옮긴다. 회색이 8.54 cm나 휜다. 파랑은 여전히 곧다.
3. 점 간격을 본다(0.25초마다 찍었다). 파란 점은 곧은 선 위에서 목표에 가까울수록 촘촘해진다. 임계 감쇠라 넘치지 않고 다가간다.

주황이 휘는 까닭은 질량을 잘못 어림했기 때문이다. 실제로는 가벼운 방향(약 5 g)으로는 너무 세게, 무거운 방향(약 22 g)으로는 너무 약하게 민 셈이다. 손끝이 가벼운 쪽으로 먼저 나가서 길이 휜다.

회색은 관절 각도를 같은 비율로 곧게 바꾼다. 10장에서 본 대로 관절 공간의 곧은 길은 손끝에서는 휜 길이다.

영상에서 보기: 59:19 목표의 위치 에너지 · 1:02:28 2관절 팔의 닫힌 루프 · 1:05:35 작업 공간의 비선형 동적 분리 · 1:07:07 JᵀF를 토크로

15.9 궤적을 따라가기, 그리고 특이점 가까이

앞먹임을 더한다

목표 하나로 가는 대신 손끝 궤적 x_d(t), ẋ_d(t), ẍ_d(t)를 따라가려면 F′를 14장의 꼴로 바꾼다.

ẍ_d는 14.5절의 앞먹임(feedforward)이다. 앞먹임이 가속을 맡고, 되먹임은 남은 오차만 고친다.

무엇의 오차를 줄이나

관절 공간에서 궤적을 따라가면 줄어드는 것은 관절 오차 e_q다. 작업 공간 제어에서는 손끝 오차 e_x가 직접 2차 식을 따라 준다. 둘의 관계는 비선형이라(작을 때만 e_x ≈ J e_q), 관절 오차가 고르게 준다고 손끝이 곧게 가지는 않는다.

손끝 오차를 직접 줄이는 것이 실제로 유리하다. 카메라로 컵을 보며 손을 옮기는 시각 서보(visual servoing)에서는 x_d가 화면에서 바로 나온다. 역기구학 없이 그 x_d를 곧장 F′에 넣으면 된다.

특이점 가까이에서

15.6절에서 팔을 펴면 Λ가 팔 방향으로 무한대로 커졌다. 그 방향으로 가속을 시키면 F = ΛF′가 크게 부풀고, 관절 토크 τ = JᵀΛF′ = MJ⁻¹F′도 J⁻¹ 때문에 치솟는다.

종이 팔을 θ₁ = 0°에 두고 팔 방향으로 100 cm/s²를 시켜 보자. 가장 큰 관절 토크는 θ₂ = 30°에서 3.1 mN·m, 10°에서 9.3 mN·m, 2°에서 46 mN·m다. 같은 가속에 30°일 때의 열다섯 배가 든다(연습문제 7).

실제로는 세 가지 길이 있다.

1. Λ⁻¹ = JM⁻¹Jᵀ는 특이점에서도 유한하다. Λ⁻¹을 계산하고, 무거운 방향의 F′를 줄이거나 버린다.
2. 8.6절의 감쇠 최소제곱처럼 J⁻¹ 대신 덜 날카로운 역을 쓴다. 그림 15-7의 시뮬레이션도 sin θ₂ < 0.1이면 이렇게 한다.
3. 특이점 근처에서는 그 방향만 모델 없는 JᵀF로 둔다.

잠깐, 생각해 보기. 모델 없는 F = −k_p(x − x_d) − k_vẋ(15.8절 처음)는 특이점에서 어떻게 되나?

Jᵀ에는 역행렬이 없어서 숫자가 터지지 않는다. 대신 팔 방향 힘은 토크를 만들지 못해서(8.9절) 그 방향으로는 움직이지 못한다. 위험하지는 않지만 느리다.

정리

다음 16강은 이 틀 위에 손끝의 단단함(컴플라이언스)과 힘 제어를 올리고, 사람 곁에서 일하는 로봇의 연구를 미리 보여 준다.

영상에서 보기: 1:04:32 특이점에서는 질량이 무한대 · 1:08:10 궤적 따라가기 · 1:09:12 손끝의 오차를 직접 · 1:09:44 e_q와 e_x

15.10 연습문제

종이 팔의 숫자(12.4절 표)를 쓴다. 손끝 힘은 mN, 토크는 mN·m이고, 1 mN = 100 g·cm/s², 1 mN·m = 10,000 g·cm²/s²다. 답은 문제 8개 아래에 있다.

1. 팔꿈치를 편 채 얼린 벽의 종이 팔(중력 토크 30.38 cos θ mN·m)에 PD만 건다. K_p = 100 mN·m/rad, 목표 θ_d = 0°다.
   1. 멈추는 각 θ*를 θ ← −30.38 cos θ / 100을 되풀이해 구하라.
   2. 중력 보상을 넣으면 θ*는?
   3. K_v = 0이면 무엇이 달라지나?
2. 가장 낮은 구조 공진이 6 Hz, 가장 긴 지연이 20 ms, 서보 루프가 200 Hz다. ω_n의 한계를 구하고, 종이 팔 어깨(M̄ = 2487 g·cm²)의 K_p와 임계 감쇠 K_v를 구하라.
3. 책상 위 종이 팔이 θ = (30°, 60°), θ̇ = (2, −1) rad/s다. τ′ = (10, −20) rad/s²를 내도록 계산 토크 τ = Mτ′ + V를 mN·m로 구하라. M(60°) = [[3086.7, 620], [620, 320]] g·cm²다.
4. θ = (0°, 90°)에서 Λ = (JM⁻¹Jᵀ)⁻¹을 이 꼴 그대로 계산해, 15.6절의 diag(5.0, 21.7) g과 같은지 확인하라.
5. 같은 자세의 종이 팔을 벽에 세운다. G = (24.5, 0) mN·m다.
   1. 손끝 하나로 팔을 받치는 힘 p = J⁻ᵀG를 구하라.
   2. 멈춘 팔에서 x − x_d = (2, −1) cm, k_p′ = 25다. F′, F = ΛF′ + p, τ = JᵀF를 구하라.
   3. τ가 M q̈ + G (q̈ = J⁻¹F′)와 같은지 확인하라.
6. 강의 예제의 RP 팔에서 m₁ = 4 kg, l₁ = 0.25 m, I_zz1 = 0.1 kg·m², m₂ = 2 kg, I_zz2 = 0.05 kg·m², d₂ = 0.5 m다. m₂′와 ¹Λ, θ₁ = 30°의 ⁰Λ를 구하라. x₀ 방향의 실효 질량은 얼마인가?
7. 종이 팔을 θ₁ = 0°에 두고 θ₂를 30°, 10°, 5°, 2°로 편다. 가장 무거운 방향의 질량과, 팔 방향으로 100 cm/s²를 낼 때의 가장 큰 관절 토크는 어떻게 변하나? (파이썬으로 구한다)
8. 그림 15-7의 처음 값을 파이썬으로 시뮬레이션해, 두 작업 공간 제어기의 길이 곧은 선에서 얼마나 벗어나는지 구하라.

답

답 1.

1. 0에서 시작하면 −0.304 → −0.290 → −0.2911 → −0.2910 rad으로 모인다. θ* ≈ −0.291 rad = −16.7°다. 그림 15-1의 해 볼 것 1과 같다.
2. 중력이 지워져 용수철만 남는다. 바닥이 목표에 있으니 θ* = 0°다.
3. 바닥은 그대로지만 에너지를 빼낼 것이 없다. 팔은 바닥 둘레로 영원히 출렁이고 멈추지 않는다. 안정하지만 점근 안정은 아니다.

답 2. 세 한계는 2π × 6 / 2 = 18.8, (2π / 0.02) / 3 = 105, 2π × 200 / 5 = 251 rad/s다. 가장 작은 ω_n = 18.8 rad/s가 한계이고, 이번에도 구조의 유연성이다.

K_p = M̄ω_n² = 2487 × 18.85² ≈ 884,000 g·cm²/s²라 88 mN·m/rad다. K_v = 2M̄ω_n = 2 × 2487 × 18.85 ≈ 93,700 g·cm²/s라 9.4 mN·m·s/rad다.

답 3. Mτ′ = (3086.7 × 10 − 620 × 20, 620 × 10 − 320 × 20) = (18,467, −200)이다. h = −600 sin 60° = −519.6이라 V = (h(1 − 4), −h × 4) = (1559, 2078)이다.

합은 τ = (20,026, 1878) g·cm²/s²이고, τ = (2.00, 0.19) mN·m다. V를 빼먹으면 팔꿈치 토크가 열 배 넘게 틀린다(−0.02 대 0.19).

답 4. M⁻¹ = (1/693,333)[[320, −320], [−320, 2486.7]]이다. 곱해 보면 JM⁻¹Jᵀ = diag(0.2, 0.0462) g⁻¹이다. 역행렬은 diag(5.0, 21.7) g으로 같다.

0.2 g⁻¹은 힘 1 g·cm/s²를 x로 걸면 가속도가 0.2 cm/s²라는 뜻이다. y로 걸면 0.046 cm/s²라서, 가벼운 x 쪽이 네 배 넘게 잘 가속한다.

답 5.

1. J⁻ᵀ = [[0, −0.125], [0.1, −0.1]]이고 G = (2450, 0) mN·cm라서 p = (0, 245) mN다. 무게로 치면 25 g쯤이다. 링크 1(20 g, 5 cm)과 링크 2(15 g, 10 cm)의 무게가 어깨에 만드는 모멘트 250 g·cm를 손끝(10 cm)이 받친다.
2. F′ = −25 × (2, −1) = (−50, 25) cm/s²다. ΛF′ = (5 × −50, 21.7 × 25) = (−250, 541.7) g·cm/s² = (−2.5, 5.42) mN이다. F = (−2.5, 250.42) mN이고, τ = JᵀF / 100 = (25.24, 0.20) mN·m다.
3. q̈ = J⁻¹F′ = (2.5, 3.75) rad/s²다. M q̈ = (7417, 2000) g·cm²/s² = (0.74, 0.20) mN·m에 G를 더하면 (25.24, 0.20)으로 같다.

답 6. m₂′ = (4 × 0.0625 + 0.1 + 0.05) / 0.25 = 1.6 kg이라 ¹Λ = diag(2, 3.6) kg이다. θ₁ = 30°이면 s₁² = 0.25, c₁² = 0.75, s₁c₁ = 0.433이다.

x₀ 방향의 실효 질량은 1/(e_xᵀΛ⁻¹e_x) = det Λ / Λ₂₂ = 7.2 / 3.2 = 2.25 kg이다. 칸 Λ₁₁ = 2.4 kg보다 가볍다. 손끝이 y₀로도 비켜 가속하면서 x₀ 방향 저항이 줄기 때문이다.

답 7. 파이썬 출력(아래 코드)을 표로 옮긴다.

질량은 1/sin²θ₂로, 토크는 대략 1/sin θ₂로 커진다. 팔을 다 펴면 둘 다 무한대다.

답 8. 파이썬 출력은 Λ 모델 0.0 cm, Λ 없이 1.67 cm다. 그림 15-7의 읽은 값과 같다. Λ 모델의 길은 계산 오차 안에서 곧은 선이다.

파이썬으로 확인

파이썬(numpy)이 있으면 아래를 붙여 넣어 돌려 본다. 주석의 값이 그대로 나온다. 단위는 g, cm, s이고, 출력할 때 mN과 mN·m로 바꾼다.

import numpy as np

np.set_printoptions(precision=2, suppress=True)
D = np.pi / 180
m1, m2, l1, l2 = 20.0, 15.0, 10.0, 8.0          # g, cm (12장과 같은 종이 팔)
lc1, lc2 = l1 / 2, l2 / 2
I1, I2 = m1 * l1**2 / 12, m2 * l2**2 / 12
g, TO_mNm = 980.0, 1e-4                          # cm/s², g·cm²/s² → mN·m

def M(q):
    c = np.cos(q[1])
    a = m1 * lc1**2 + I1 + m2 * (l1**2 + lc2**2 + 2 * l1 * lc2 * c) + I2
    b = m2 * (lc2**2 + l1 * lc2 * c) + I2
    return np.array([[a, b], [b, m2 * lc2**2 + I2]])

def V(q, qd):                                    # 12.8절
    h = -m2 * l1 * lc2 * np.sin(q[1])
    return np.array([h * (qd[1]**2 + 2 * qd[0] * qd[1]), -h * qd[0]**2])

def G(q):                                        # 벽에 세운 팔 (12.9절)
    c1, c12 = np.cos(q[0]), np.cos(q[0] + q[1])
    return np.array([(m1 * lc1 + m2 * l1) * g * c1 + m2 * lc2 * g * c12, m2 * lc2 * g * c12])

def fk(q):
    return np.array([l1 * np.cos(q[0]) + l2 * np.cos(q[0] + q[1]),
                     l1 * np.sin(q[0]) + l2 * np.sin(q[0] + q[1])])

def J(q):
    s1, c1, s12, c12 = np.sin(q[0]), np.cos(q[0]), np.sin(q[0] + q[1]), np.cos(q[0] + q[1])
    return np.array([[-l1 * s1 - l2 * s12, -l2 * s12], [l1 * c1 + l2 * c12, l2 * c12]])

def Jdot_qdot(q, qd):                            # q̈ = 0일 때 손끝의 가속도 (원을 그리는 몫)
    w1, w12 = qd[0], qd[0] + qd[1]
    return np.array([-l1 * np.cos(q[0]) * w1**2 - l2 * np.cos(q[0] + q[1]) * w12**2,
                     -l1 * np.sin(q[0]) * w1**2 - l2 * np.sin(q[0] + q[1]) * w12**2])

def Lam(q):                                      # Λ = (J M⁻¹ Jᵀ)⁻¹ (단위 g)
    Jq = J(q)
    return np.linalg.inv(Jq @ np.linalg.solve(M(q), Jq.T))

def felt_mass(L, deg):                           # 그 방향으로 밀 때 느끼는 질량 1/(uᵀΛ⁻¹u)
    u = np.array([np.cos(deg * D), np.sin(deg * D)])
    return 1 / (u @ np.linalg.solve(L, u))

# 문제 1: 팔꿈치를 편 채 벽에 세운 팔, PD만 (중력 보상 없음)
A, Kp = 30.38, 100.0                             # G(θ) = A cos θ, Kp는 mN·m/rad
th = 0.0
for _ in range(20):
    th = -A * np.cos(th) / Kp                    # Kp(θ − 0) + A cos θ = 0 을 되풀이로 푼다
print(round(th, 4), round(th / D, 1))            # -0.291 -16.7
# 문제 2: 이득의 한계 (구조 공진 6 Hz, 지연 20 ms, 서보 루프 200 Hz)
wn = min(2 * np.pi * 6 / 2, 2 * np.pi / 0.02 / 3, 2 * np.pi * 200 / 5)
Mbar = M(np.array([0, 90 * D]))[0, 0]
print(round(wn, 1), round(Mbar * wn**2 * TO_mNm, 1), round(2 * Mbar * wn * TO_mNm, 2))   # 18.8 88.4 9.37
# 문제 3: 계산 토크 τ = M τ′ + V (책상 위)
q, qd, tp = np.array([30, 60]) * D, np.array([2.0, -1.0]), np.array([10.0, -20.0])
tau = M(q) @ tp + V(q, qd)
print(tau.round(0), (tau * TO_mNm).round(2))     # [20026.  1878.] [2.   0.19]
# 15.6, 문제 4: θ = (0°, 90°)의 Λ
q0 = np.array([0, 90]) * D
print(J(q0), np.linalg.inv(J(q0)))               # [[-8. -8.] [10.  0.]] [[ 0.    0.1 ] [-0.12 -0.1 ]]
X = J(q0) @ np.linalg.solve(M(q0), J(q0).T)      # J M⁻¹ Jᵀ (단위 1/g)
print([float(round(v, 4)) for v in np.diag(X)], abs(X[0, 1]) < 1e-12)   # [0.2, 0.0462] True
print(Lam(q0))                                   # [[ 5.    0.  ] [ 0.   21.67]]
Ji = np.linalg.inv(J(q0))
print(np.allclose(Ji.T @ M(q0) @ Ji, Lam(q0)))   # True  (J⁻ᵀ M J⁻¹ = (J M⁻¹ Jᵀ)⁻¹)
print(round(felt_mass(Lam(q0), 45), 2))          # 8.12  45° 방향으로 밀 때
# 문제 5: 벽에서 손끝으로 버티는 힘 p = J⁻ᵀG, 그리고 F = ΛF′ + p, τ = JᵀF
p = np.linalg.solve(J(q0).T, G(q0))              # g·cm/s²
print((p / 100).round(1) + 0.0)                  # [  0. 245.]  mN
Fp = -25 * np.array([2.0, -1.0])                 # F′ = −k_p′(x − x_d), 멈춘 상태
F = Lam(q0) @ Fp + p
print((F / 100).round(2), (J(q0).T @ F * TO_mNm).round(2))       # [ -2.5  250.42] [25.24  0.2 ]
qdd = np.linalg.solve(J(q0), Fp)                 # 멈춘 상태: q̈ = J⁻¹ F′
print(qdd, ((M(q0) @ qdd + G(q0)) * TO_mNm).round(2))            # [2.5  3.75] [25.24  0.2 ]
# 15.6: μ = J⁻ᵀV − Λh, h = J̇q̇ 확인 (아무 자세·속도에서 JᵀF = τ가 되는지)
q, qd, qdd = np.array([0.4, 1.2]), np.array([1.5, -2.0]), np.array([3.0, 1.0])
tau = M(q) @ qdd + V(q, qd)                      # 책상 위
xdd = J(q) @ qdd + Jdot_qdot(q, qd)
mu = np.linalg.solve(J(q).T, V(q, qd)) - Lam(q) @ Jdot_qdot(q, qd)
print(np.allclose(J(q).T @ (Lam(q) @ xdd + mu), tau))            # True
# 문제 6: 강의 예제 RP 팔 (kg, m)
m1r, l1r, I1r, m2r, I2r, d2 = 4.0, 0.25, 0.1, 2.0, 0.05, 0.5
m2p = (m1r * l1r**2 + I1r + I2r) / d2**2
L1r = np.diag([m2r, m2r + m2p])
c, s = np.cos(30 * D), np.sin(30 * D)
R = np.array([[c, -s], [s, c]])
L0r = R @ L1r @ R.T
print(round(m2p, 2), L0r, round(felt_mass(L0r, 0), 2))           # 1.6 [[ 2.4  -0.69] [-0.69  3.2 ]] 2.25
# 문제 7: 팔을 펴 갈수록 무거워지는 방향 (θ₁ = 0)
for t2 in (30, 10, 5, 2):
    q = np.array([0, t2]) * D
    big = np.linalg.eigvalsh(Lam(q))[1]
    u = fk(q) / np.linalg.norm(fk(q))            # 팔 방향 (어깨 → 손끝)
    qdd = np.linalg.solve(J(q), 100 * u)         # 팔 방향으로 100 cm/s² (멈춘 상태, 책상 위)
    print(t2, round(big), round(big * np.sin(t2 * D)**2, 1), round(np.abs(M(q) @ qdd).max() * TO_mNm, 2))
# 30 57 14.3 3.1 / 10 397 12.0 9.27 / 5 1546 11.7 18.53 / 2 9589 11.7 46.32

# 문제 8: 작은 시뮬레이션 (책상 위, 3초, 룽게-쿠타 4단)
def ctrl(mode, q, qd, xd, wn=4.0, mbar=12.5):
    kp, kv = wn**2, 2 * wn
    x, xdot = fk(q), J(q) @ qd
    Fp = -kp * (x - xd) - kv * xdot              # 단위 질량 제어 F′
    if mode == 'lam':                            # F = ΛF′ + μ
        mu = np.linalg.solve(J(q).T, V(q, qd)) - Lam(q) @ Jdot_qdot(q, qd)
        return J(q).T @ (Lam(q) @ Fp + mu)
    return J(q).T @ (mbar * Fp)                  # Λ 없이: 질량을 12.5 g로 어림

def simulate(mode, q0, xd, T=3.0, h=0.002):
    s = np.r_[q0, 0, 0]
    f = lambda s: np.r_[s[2:], np.linalg.solve(M(s[:2]), ctrl(mode, s[:2], s[2:], xd) - V(s[:2], s[2:]))]
    path = [fk(s[:2])]
    for _ in range(int(round(T / h))):
        k1 = f(s); k2 = f(s + h / 2 * k1); k3 = f(s + h / 2 * k2); k4 = f(s + h * k3)
        s = s + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
        path.append(fk(s[:2]))
    return np.array(path)

q0, xd = np.array([-10, 100]) * D, np.array([-6.0, 12.0])
x0 = fk(q0)
nrm = np.array([-(xd - x0)[1], (xd - x0)[0]]) / np.linalg.norm(xd - x0)
for mode in ('lam', 'pd'):
    path = simulate(mode, q0, xd)
    print(mode, round(np.abs((path - x0) @ nrm).max(), 2), path[-1].round(2))
# lam 0.0 [-6. 12.] / pd 1.67 [-5.99 11.99]

스탠퍼드 숙제

스탠퍼드 강의 페이지의 Homework 6에서 2번이 이 장의 관절 공간 부분이다. (a)·(b)는 15.1절처럼 자세마다 바뀌는 관성 앞에서 고정 이득 PD가 어떻게 달라지는지 묻고, (c)·(d)는 15.4절의 τ = ατ′ + β 궤적 제어와 정상 상태 오차를 구한다.

1번(질량 하나의 PD와 나누기)은 13~14장의 내용이다. 작업 공간 제어는 숙제에 없으니 위 연습문제 4~8로 익힌다. 풀이는 같은 페이지의 Solution 6에 있다.

다 풀었으면 15강 영상을 한 번 본다.