10장. 부드러운 길: 궤적 만들기

팔을 여기서 저기로 옮길 때 "언제 어디를 지나는지"를 정하는 것이 궤적이다. 이 장은 관절 각도를 3차 다항식과 "직선 + 포물선 혼합"으로 부드럽게 잇는 법을 배운다. 그래서 모터가 덜컥거리지 않는다. 강의 10강(62분)에 해당한다.

10.1 무엇을 정하나: 길과 시간표

지금까지 배운 것은 모두 한 순간의 질문이었다.

• 이 자세에서 손끝은 어디인가? (순기구학)
• 손끝을 거기 두려면 몇 도인가? (역기구학)
• 관절이 이 빠르기로 돌면 손끝은 얼마나 빠른가? (자코비안)

이 장은 시간을 넣는다. 손끝을 여기서 저기로 몇 초 동안 어떻게 옮길까?

이 강의는 강의 노트를 함께 쓴 Krasimir Kolarov 박사가 맡는다. 자율 주행 차 영상 이야기로 시작하는데, 차도 결국 "다른 물체 사이로 길 짜기"를 한다는 것이 요점이다.

출발점, 경유점, 도착점

종이 팔로 예를 들자. 손끝이 A에서 C로 가야 하는데 가운데에 컵이 있다. 곧장 가면 컵에 부딪히니, 컵 안쪽의 B를 거쳐 돌아간다.

• 경로 점(path points): 지나야 할 점들이다. 처음의 출발점(initial point), 끝의 도착점(final point), 그 사이의 경유점(via point)이 있다.
• 경로(path): 손끝이 지나가는 곳의 모양이다. 시간은 들어 있지 않다.
• 궤적(trajectory): 경로에 시간을 붙인 것이다. 자유도마다 위치, 속도, 가속도를 시간에 대해 적은 표다.

그림 10-1 아래가 바로 궤적이다. 어깨 θ₁과 팔꿈치 θ₂마다 "몇 초에 몇 도"인 곡선이 하나씩 있다. 1.5초에 B, 3초에 C를 지난다.

지켜야 할 것 세 가지

1. 공간: 장애물에 부딪히지 않는다. 경유점은 대개 이것 때문에 넣는다.
2. 시간: 정해진 시간 안에 끝낸다. 공장의 컨베이어 옆에서는 한 동작에 쓸 시간이 정해져 있다.
3. 부드러움: 덜컥거리지 않는다. 부드럽게 움직이면 에너지를 덜 쓰고, 제어하기도 쉽다.

잠깐, 생각해 보기. 도착만 하면 되는데, 왜 속도와 가속도까지 정해야 할까?

모터가 내야 하는 힘(토크)은 가속도에 비례한다. 뉴턴의 F = ma와 같다(11~12강 동역학).

가속도가 순간적으로 튀면 모터도 순간적으로 큰 힘을 내야 한다. 서보가 덜컥하고, 팔이 떨리고, 기어가 상한다.

관절마다 따로, 시간은 함께

강의는 자유도마다 궤적을 따로 짠다. 대신 모든 관절이 같은 시각에 출발하고 같은 시각에 도착하게 맞춘다. 그러면 팔 전체가 한 동작으로 움직인다.

그래서 이 장의 식은 숫자 하나에 대한 식이다. 강의는 그 숫자를 u라는 글자로 쓴다.

u는 관절 각도일 수도, 손끝의 x나 y일 수도, 손 방향의 각도일 수도 있다. 이 장은 주로 관절 각도 θ로 쓴다.

영상에서 보기: 01:43 출발에서 도착까지, 경유점을 거쳐 · 02:47 궤적 = 위치·속도·가속도의 시간표 · 03:18 공간, 시간, 부드러움 · 12:09 변수 u 하나로

10.2 관절 공간에서 짤까, 작업 공간에서 짤까

경로 점은 보통 손끝의 위치로 주어진다. "컵 위 5 cm"처럼 말이다. 이것을 잇는 방법은 두 가지다.

1. 관절 공간 계획(joint-space planning): 경로 점마다 역기구학을 한 번씩 풀어 관절 각도로 바꾼다. 그다음 관절 각도 사이를 부드럽게 잇는다. 역기구학은 경로 점 개수만큼만 푼다.
2. 작업 공간 계획(Cartesian-space planning): 손끝 위치를 직접 잇는다. 예를 들어 직선으로 잇는다. 움직이는 동안 짧은 간격마다 역기구학을 풀어 관절 각도를 구한다.

강의는 둘째를 "Cartesian 공간"이라 부른다. 이 교재의 작업 공간(operational space, 2.2절)과 같은 판이다.

같은 두 점, 다른 두 길

종이 팔의 손끝을 A = (15, 0) cm에서 B로 옮긴다. B도 받침에서 15 cm 떨어져 있고, 방향만 다르다. 두 방법 모두 팔꿈치가 같은 쪽으로 꺾인 답(θ₂ < 0, 5.9절)을 쓴다.

관절 공간의 길. A에서는 θ = (29.5°, −67.6°)이고, B가 90° 방향이면 θ = (119.5°, −67.6°)다. 두 자세의 θ₂가 같으니 θ₁만 29.5°에서 119.5°로 고르게 돌리면 된다. 손끝은 반지름 15 cm인 원호를 그린다.

작업 공간의 직선. 손끝은 A에서 B로 곧게 간다. 그 대신 가운데 (7.5, 7.5)에서 θ = (90.6°, −108.8°)가 된다. 팔꿈치가 −67.6°에서 −108.8°까지 접혔다가 다시 펴진다.

잠깐, 생각해 보기. 관절 공간의 길에서 손끝이 정확히 원을 그리는 까닭은 무엇일까?

받침에서 손끝까지의 거리 r은 θ₂만으로 정해진다. 코사인 법칙으로 r² = 164 + 160 cos θ₂였다(5.9절, 7.10절). θ₂가 그대로면 r도 그대로 15 cm다. θ₁만 고르게 도니, 손끝은 원 위를 고르게 돈다.

슬라이더로 해 볼 것

1. 목표 B의 방향 90°, 진행 50%: 두 손끝 점 사이가 15 − 10.6 = 4.4 cm다. 손끝이 지나는 곳이 꽤 다르다.
2. 방향 30°: 두 길이 거의 겹친다. 짧게 움직일 때는 관절 공간으로 짜도 손끝이 거의 곧게 간다.
3. 아래 숫자 "진행 1%마다 도는 각도"를 본다. 관절 공간은 늘 고르지만, 직선은 어떤 곳에서 관절이 더 빨리 돌아야 한다. 90°에서는 팔꿈치가 1%마다 최대 1.74°를 돈다.

이 마지막 숫자가 다음 절의 주제다. 방향을 150°, 160°, 170°로 늘려 보자.

알아 두기. 실제 로봇은 둘을 섞어 쓴다. 넓은 빈 곳은 관절 공간으로 빠르게 지나간다. 물건 가까이에서는 작업 공간으로 곧게 움직인다(예: 구멍에 핀 꽂기). 손 방향은 3장의 오일러 각이나 각-축 표현의 각도를 위치와 같은 방법으로 잇는다.

영상에서 보기: 04:19 두 가지 공간 · 05:55 관절 공간은 직선을 못 따른다 · 06:59 작업 공간은 모양을 따른다 · 07:31 대신 실행 중 계산이 많다

10.3 작업 공간 계획의 함정 세 가지

작업 공간에서 짠 직선은 보기에 좋다. 그런데 길 가운데에서 역기구학이 실패할 수 있다. 미리 길 전체를 확인하지 않으면 움직이는 도중에야 알게 된다.

강의는 관절 2개인 팔로 세 가지 경우를 보여 준다.

(가) 길 가운데가 닿지 않는다

A와 B는 둘 다 닿는 곳에 있다. 그런데 둘을 잇는 직선이 닿지 않는 곳을 지난다. 종이 팔은 받침에서 2 cm 안쪽에 손끝이 갈 수 없다(5.9절).

그림 10-2에서 직선이 받침에 가장 가까이 가는 거리는 d = 15 cos(φ/2)다. φ는 목표 B의 방향이다. d < 2 cm가 되는 φ > 164.7°에서는 직선 가운데가 고리 안쪽 구멍을 지난다.

슬라이더를 170°에 두고 진행을 50% 근처로 옮겨 보자. "닿지 않음"이 뜬다.

(나) 특이점 가까이 지난다

이번에는 길 전체가 닿는 곳 안에 있다. 그런데 길이 특이점 근처를 지나면 관절 속도가 끝없이 커진다(7.10절). 손끝 속도는 그대로인데 관절이 따라가지 못하고, 손끝이 직선에서 벗어난다.

종이 팔의 다 접은 자세(θ₂ = ±180°, r = 2 cm)가 특이점이다. 그림 10-2에서 방향을 160°로 해 보자. 직선은 받침에서 2.6 cm까지 다가간다.

164°에서는 직선을 따라가는 어깨가 관절 공간보다 7배 빨리 돌아야 하는 순간이 있다. 그때 팔꿈치는 −176°까지 접혀 특이점에 바짝 붙는다. 손끝이 받침 바로 옆을 지나니 받침에서 본 손끝의 방향도 확 바뀐다.

(다) 출발점과 도착점이 다른 자세 가지에 있다

종이 팔은 같은 손끝에 두 자세로 닿는다(5.9절). 보통은 둘 중 아무거나 써도 된다. 그런데 서보가 돌 수 있는 범위가 정해져 있으면 이야기가 달라진다.

어깨 서보가 0°~180°만 돈다고 하자. A = (15, 4)에 닿는 두 답은 (41.8°, −61.2°)와 (−11.9°, 61.2°)다. 둘째는 범위 밖이니 A는 파란 자세(θ₂ < 0)로만 닿는다. 좌우가 뒤집힌 B = (−15, 4)는 주황 자세(θ₂ > 0)로만 닿는다.

직선 위의 점은 모두 어느 한 자세로는 닿는다. 그런데 파란 자세로 가다 보면 x = −3.1 cm에서 어깨가 180°를 넘는다. 그 전에 주황 자세로 갈아타야 한다.

갈아타는 일은 한순간에 할 수 없다. x = 0에서 파란 자세는 (139.5°, −157.7°), 주황 자세는 (40.5°, 157.7°)다.

두 자세는 θ₂ = 0°(쭉 편 팔)이나 ±180°(다 접은 팔)에서만 만나는데, 이 직선은 그런 곳(r = 18 cm나 2 cm)을 지나지 않는다. 손끝이 직선에서 벗어나 팔을 크게 바꿔 접어야 한다.

알아 두기. 그래서 작업 공간 계획은 움직이기 전에 길 전체를 확인한다. 길 위에 점을 촘촘히 찍고 점마다 역기구학을 풀어 본다. 닿는지, 관절 범위 안인지, 관절 속도가 한계 안인지 본다. 하나라도 걸리면 경유점을 바꾸거나 관절 공간으로 짠다.

관절 공간 계획에는 이런 함정이 없다. 경로 점의 자세를 처음부터 관절 각도로 정해 두고, 그 사이를 관절 각도로 잇기 때문이다. 대신 손끝이 어디로 지나는지는 정할 수 없다.

영상에서 보기: 09:03 두 링크 팔의 어려움 · 10:04 가운데 점이 닿지 않는다 · 10:36 특이점 근처 · 11:07 서로 다른 자세 가지

10.4 3차 다항식: 조건 네 개, 계수 네 개

이제 한 관절의 각도를 시간에 대한 곡선으로 만든다. θ₀에서 출발해 T초 뒤에 θ_f에 도착한다. 숙제와 교재는 걸리는 시간 T를 t_f로 쓰기도 한다.

직선(1차식)은 왜 안 되나

가장 쉬운 길은 직선이다. θ(t) = a₀ + a₁t이고, 출발 위치와 도착 위치 두 조건으로 a₀, a₁이 정해진다. 속도는 내내 a₁이다.

문제는 출발하는 순간이다. 멈춰 있던 관절의 속도가 0에서 a₁로 한순간에 바뀐다. 가속도가 무한대라는 뜻이다.

실제 서보는 그렇게 못 하고 덜컥거린다. 경유점에서도 앞뒤 직선의 기울기가 다르면 같은 일이 생긴다.

그래서 계수가 더 많은 곡선을 쓴다. 강의가 고르는 후보는 넷이다.

1. 직선: 경로 점에서 속도가 끊긴다.
2. 직선 + 혼합: 경로 점 근처만 부드럽게 꺾는다(10.6절).
3. 3차 다항식: 이 절.
4. 더 높은 차수(5차 등): 가속도까지 맞춘다(10.8절).

3차 다항식과 그 미분

계수가 네 개라서 조건 네 개를 맞출 수 있다. 출발 위치, 도착 위치, 출발 속도, 도착 속도다.

수학 더하기: tⁿ의 미분. 위치를 시간으로 미분하면 속도, 속도를 미분하면 가속도다(6장에서 글자 위의 점으로 썼다). 다항식은 항마다 규칙 하나로 미분한다. tⁿ을 미분하면 n tⁿ⁻¹이다. 예: t³ → 3t², t² → 2t, t → 1, 숫자 → 0.

멈춤에서 멈춤으로

가장 흔한 경우는 정지 상태에서 출발해 정지 상태로 도착하는 것이다. 조건 네 개는 이렇다.

t = 0을 넣으면 t가 붙은 항이 모두 사라진다. 그래서 첫째 조건에서 a₀ = θ₀, 셋째 조건에서 a₁ = 0이 바로 나온다. 남은 두 조건에는 t = T를 넣고, Δ = θ_f − θ₀라 쓴다.

수학 더하기: 연립일차방정식. 미지수 두 개에 식 두 개면 한 식을 한 미지수로 풀어 다른 식에 넣는다(대입). 오른쪽 식에서 a₂ = −(3/2)a₃T다. 왼쪽 식에 넣으면 −(3/2)a₃T³ + a₃T³ = −(1/2)a₃T³ = Δ다. 그래서 a₃ = −2Δ/T³이고, 다시 넣으면 a₂ = 3Δ/T²다.

다항식이 좋은 점이 이것이다. 계수에 대해 식이 모두 일차라서, 조건이 몇 개든 연립일차방정식을 풀면 된다.

예: 종이 팔을 3초 동안 움직이기

어깨를 0°에서 90°로, 팔꿈치를 −90°에서 −30°로 3초 동안 옮긴다. 어깨는 Δ = 90°, 팔꿈치는 Δ = 60°다. 공식에 넣는다.

세 그래프에서 읽을 것:

1. 각도는 천천히 출발해 가운데서 빠르고, 끝에서 천천히 선다. S자다.
2. 속도는 언덕 모양이고, 한가운데 t = T/2에서 가장 빠르다. 어깨는 1.5초에 45°/s다. 일반적으로 최고 속도는 1.5Δ/T다.
3. 가속도는 비스듬한 직선이다. 처음에 6Δ/T²(어깨 60°/s²)로 밀고, 가운데서 0, 끝에서 −6Δ/T²로 세운다.

두 관절은 같은 시각에 출발하고 같은 시각에 선다. 각도의 크기만 다를 뿐 곡선 모양이 똑같다. 그래서 손끝은 점선을 따라 한 번에 부드럽게 간다.

잠깐, 생각해 보기. 같은 90°를 3초가 아니라 1.5초에 가게 하면 최고 속도와 가속도는 몇 배가 될까?

최고 속도 1.5Δ/T는 T에 반비례하니 2배(90°/s)다. 가속도 6Δ/T²는 T²에 반비례하니 4배(240°/s²)다. 빨리 가려면 모터가 낼 힘이 제곱으로 커진다.

거꾸로, 서보의 최고 속도와 최고 가속도를 알면 가장 짧은 T를 구할 수 있다(문제 2).

가속도는 고를 수 없다

강의 중에 학생이 묻는다. "가속도가 상수라는 말인가요?"

교수의 답은 이렇다. 가속도는 직선이라 시간에 따라 바뀐다. 다만 조건 네 개를 위치와 속도에 다 썼으니, 가속도를 원하는 값으로 정할 수는 없다.

예를 들어 "출발 순간의 가속도는 0으로" 같은 조건은 넣을 자리가 없다. 그림 10-4에서 t = 0을 보면, 정지해 있던 관절의 가속도가 갑자기 60°/s²로 시작한다. 이것을 고치려면 계수가 더 필요하다(10.8절).

영상에서 보기: 13:12 직선은 속도가 끊긴다 · 17:52 3차 다항식 · 19:25 정지에서 정지로 · 22:36 가속도는 고를 수 없다

10.5 경유점 지나기: 속도를 어떻게 정할까

경유점이 있으면 구간마다 3차 다항식을 하나씩 쓴다. 경유점마다 멈췄다 가면 10.4절의 식을 그대로 쓰면 된다.

하지만 경유점은 대개 장애물을 피하려고 넣은 것이다. 거기서 멈출 까닭이 없고, 멈추면 시간과 에너지를 버린다.

끝 속도가 0이 아닌 3차 다항식

멈추지 않으려면 경유점에서의 속도가 0이 아니어야 한다. 출발 속도 θ̇₀와 도착 속도 θ̇_f가 주어졌다고 하자. 조건은 여전히 네 개다. a₀ = θ₀, a₁ = θ̇₀이고, 남은 두 식을 같은 방법으로 풀면 이렇다.

θ̇₀ = θ̇_f = 0을 넣으면 10.4절의 답으로 돌아간다. 이제 남은 질문은 "경유점의 속도를 몇으로 하나"다.

속도를 정하는 세 가지 방법

1. 손끝 속도로 정한다. 경유점에서 손끝이 낼 속도를 알면, 자코비안의 역행렬로 관절 속도를 구한다. θ̇ = J⁻¹ẋ다(6~7장).
2. 어림 규칙(heuristic)으로 정한다. 경유점 앞뒤 두 구간의 평균 기울기를 본다. 부호가 같으면 둘의 평균을, 부호가 바뀌면(올라가다 내려가면) 0을 쓴다.
3. 가속도까지 이어지게 정한다. 경유점마다 "앞 구간 끝 가속도 = 뒤 구간 처음 가속도"라는 조건을 세우고, 속도를 연립방정식으로 구한다.

예: 경유점 두 개, 구간 세 개

어깨 하나를 A(0°) → B(30°) → C(90°) → D(60°)로, 1초 간격으로 움직인다. 구간의 평균 기울기는 30°/s, 60°/s, −30°/s다.

어림 규칙으로 B의 속도는 (30 + 60)/2 = 45°/s다. C에서는 60에서 −30으로 부호가 바뀌니 0이다.

구간마다 위 공식에 넣으면 세 식이 나온다. t는 구간마다 0부터 다시 잰다.

슬라이더로 해 볼 것

1. 기본값(어림 규칙, 45와 0): 곡선이 경유점을 부드럽게 지난다. 속도도 끊기지 않는다. 그런데 가속도는 B에서 0 → 180°/s², C에서 −270 → −180°/s²로 튄다.
2. 두 속도를 모두 0으로: 경유점마다 멈췄다 간다. BC 구간의 최고 속도가 90°/s로, 어림 규칙의 81°/s보다 빠르다. 멈추느라 남은 시간에 더 서둘러야 한다.
3. C의 속도를 40으로: 각도가 90°를 넘어 92.5°까지 갔다가 돌아온다. 부호가 바뀌는 곳에서 0을 쓰는 까닭이다.
4. B를 66, C를 6으로: 가속도의 튐이 사라진다. 아래에서 이 숫자를 구한다.

잠깐, 생각해 보기. C에서 속도를 0으로 하면, 관절이 C에서 멈춰 서는 것일까?

한순간만 속도가 0이다. 공을 위로 던지면 꼭대기에서 한순간 멈췄다가 떨어지는 것과 같다. 속도는 끊기지 않고 양수에서 음수로 이어지니, 경유점마다 정지하는 2번과는 다르다.

가속도까지 잇기

방법 3을 계산해 보자. 앞 구간의 끝 가속도와 뒤 구간의 처음 가속도를 위 공식으로 쓰고 같다고 놓는다. 구간 시간이 모두 h로 같으면 경유점 i마다 이런 식이 나온다.

예제는 h = 1이고, 처음과 끝의 속도가 0이다. B에서 4θ̇_B + θ̇_C = 3 × 90 = 270, C에서 θ̇_B + 4θ̇_C = 3 × 30 = 90이다. 풀면 θ̇_B = 66°/s, θ̇_C = 6°/s다.

이렇게 이은 3차 다항식 묶음을 3차 스플라인(cubic spline)이라 한다.

대가도 있다. 조건을 경유점에 쓰고 나니 처음과 끝의 가속도는 고를 수 없다(이 예에서는 48°/s²로 출발해 192°/s²로 끝난다). 강의 말대로 조건을 더하면 다른 조건을 빼야 한다.

영상에서 보기: 21:34 경유점에서는 멈추지 않는다 · 26:14 경유점 속도는 어떻게 · 27:16 어림 규칙 · 28:20 가속도도 이어지게

10.6 직선과 포물선 섞기

3차 다항식은 내내 속도가 바뀐다. 실제 작업에서는 "일정한 속도로 쭉 가는" 움직임이 편할 때가 많다. 컨베이어를 따라가거나, 모터를 최고 속도 근처로 오래 쓰고 싶을 때다.

직선(일정한 속도)을 쓰되, 속도가 끊기는 양 끝만 고친다.

1. 처음 t_b 동안은 일정한 가속도로 속도를 올린다.
2. 가운데는 일정한 속도로 간다.
3. 마지막 t_b 동안은 같은 크기로 속도를 줄인다.

가속도가 일정한 구간의 각도는 포물선이 된다. 그래서 이것을 포물선 혼합(parabolic blend)이라 하고, t_b를 혼합 시간이라 한다.

혼합 시간 구하기

양 끝의 혼합 시간을 같게 두면 움직임이 좌우 대칭이다. 그러면 직선 구간이 한가운데 점(T/2, (θ₀ + θ_f)/2)을 지난다. 혼합이 끝나는 순간의 속도 a t_b가 그 직선의 기울기와 같아야 한다.

정리하면 t_b에 대한 2차 방정식이 나온다.

수학 더하기: 근의 공식. pt² + qt + r = 0의 답은 t = (−q ± √(q² − 4pr)) / (2p)다. 여기서는 p = a, q = −aT, r = Δ다. 혼합 시간은 T/2보다 길 수 없으니 −를 고른다.

루트 안이 음수면 답이 없다. 그러니 가속도는 a ≥ 4Δ/T²이어야 한다. 이보다 약한 모터로는 T초 안에 Δ만큼 못 간다.

a = 4Δ/T²이면 t_b = T/2라서 직선 구간이 없다. 가속하자마자 감속한다.

거꾸로 t_b를 먼저 고르면 가속도와 직선 구간의 속도가 바로 나온다.

예: 90°를 3초에

10.4절과 같이 Δ = 90°, T = 3 s다. 가속도를 a = 72°/s²로 정하면 루트 안이 72² × 9 − 4 × 72 × 90 = 20736, 루트는 144다. t_b = 1.5 − 144/144 = 0.5 s, 직선 구간 속도는 72 × 0.5 = 36°/s다.

이 움직임에 필요한 가장 작은 가속도는 4 × 90/9 = 40°/s²다.

해 볼 것:

1. t_b를 0.3 s로 줄인다. 직선 구간이 길어지고 속도는 33.3°/s로 낮지만, 가속도가 111°/s²로 커진다. 직선(1차식)에 가까워질수록 가속도가 커진다.
2. t_b = 1 s: 최고 속도가 45°/s로 3차식과 같다. 그런데 가속도는 45°/s²로, 3차식의 60보다 작다.
3. t_b = 1.5 s: 직선 구간이 사라진다. 가속도는 가장 작은 40°/s²지만 최고 속도는 60°/s로 가장 크다.

혼합 시간을 늘리면 가속도는 줄고 최고 속도는 는다. 모터의 속도 한계와 힘(가속도) 한계 사이에서 고르는 것이다.

알아 두기. 속도 그래프는 사다리꼴이라 사다리꼴 속도 프로파일(trapezoidal velocity profile)이라고도 부른다. 산업용 로봇과 모터 드라이버가 흔히 쓰는 방식이다. 최고 속도와 최고 가속도만 알면 바로 짤 수 있기 때문이다.

영상에서 보기: 29:54 직선(1차식) · 30:56 포물선 혼합 · 33:03 속도가 이어지는 조건

10.7 경유점이 여러 개일 때의 직선 + 혼합

경유점이 여럿이면 경유점 사이를 직선으로 잇고, 꺾이는 곳마다 포물선 혼합을 넣는다. 강의는 이 식들을 빠르게 지나간다. 여기서는 무엇이 주어지고 무엇을 구하는지 정리하고, 숫자로 따라가 본다.

• 주어진 것: 경로 점의 각도 θ_j, 구간마다 원하는 시간 t_djk, 혼합 구간에서 쓸 가속도의 크기 A.
• 구할 것: 혼합 시간 t_k, 직선 구간 시간 t_jk, 직선 구간의 속도 v_jk, 부호까지 붙은 가속도 a_k.

10.6절처럼 속도는 v, 가속도는 a로 쓴다. Craig 교재와 강의 노트는 같은 것을 θ 위에 점 하나(속도), 점 둘(가속도)을 찍어 쓴다.

가속도의 크기는 보통 관절 모터의 한계로 정한다. 구간 시간은 기본 속도로 거리를 나누어 정할 수도 있다.

가운데 경유점의 식

경유점 j, k, l이 차례로 있다고 하자. 직선 구간의 속도는 그냥 "각도 차이 ÷ 시간"이다. 경유점 k에서 앞 직선의 속도를 뒤 직선의 속도로 바꾸는 데 혼합 구간을 쓴다.

수학 더하기: 부호 함수 sgn. sgn(x)는 x가 양수면 1, 음수면 −1, 0이면 0이다. 속도를 높여야 하면 가속도를 +로, 낮춰야 하면 −로 고르는 데 쓴다.

혼합 구간은 경유점 시각의 앞뒤로 절반씩 걸친다. 그래서 직선 구간의 시간은 구간 시간에서 양쪽 혼합의 절반씩을 뺀 것이다.

첫 구간과 마지막 구간

양 끝에서는 정지 상태에서 출발하고 정지 상태로 선다. 그래서 혼합 구간 전체가 구간 안쪽에 들어간다. 첫 구간의 식은 이렇다.

마지막 구간도 같은 모양이다. 거꾸로 시간을 되감아 보면 마지막 구간이 첫 구간처럼 보이기 때문이다. Craig 교재의 식 (7.24)~(7.28)에 모두 있고, 10.11절의 파이썬 코드가 그대로 계산한다.

예: 10.5절과 같은 경유점, 가속도 150°/s²

A(0°) → B(30°) → C(90°) → D(60°), 구간마다 1초, 가속도의 크기 A = 150°/s²다.

첫 구간은 t₁ = 1 − √(1 − 60/150) = 1 − √0.6 = 0.225 s이고, 직선 속도는 30/(1 − 0.113) = 33.8°/s다. 가운데 BC는 60/1 = 60°/s다. C에서는 60에서 −33.8로 바꾸니 t = 93.8/150 = 0.625 s가 걸린다.

시간을 모두 더하면 0.225 + 0.687 + 0.175 + 0.600 + 0.625 + 0.462 + 0.225 = 3.0 s다. 원하던 시간과 맞는다.

경유점을 지나지 않는다

그림을 보면 곡선이 C(90°)에 닿지 않고 83.2°에서 돈다. C의 혼합 구간이 C의 시각 앞뒤에 걸쳐 있어서, 직선이 C에 닿기 전에 미리 꺾기 시작하기 때문이다. 강의에서도 학생이 이 점을 묻는다.

교수의 답은 이렇다. 경유점은 대개 장애물을 피하려고 넣은 것이라 정확히 지날 필요가 없다. 꼭 지나야 한다면 방법이 있다.

1. 가속도를 크게 한다. 혼합 구간이 짧아져 경유점에 더 가까이 간다. 가속도 크기를 400°/s²로 하면 최고 각도가 87.7°가 된다.
2. 유사 경유점(pseudo via point)을 넣는다. 진짜 경유점 앞뒤에 점을 하나씩 더 둔다. 그 사이의 직선 구간이 진짜 경유점을 지나게 고른다.
3. 그 점에서 멈추고 싶으면 경유점을 두 번 넣는다. 같은 점 사이의 구간은 속도가 0이라 거기 멈춘다.

알아 두기. 3차 다항식(10.5절)은 경유점을 정확히 지나고, 직선 + 혼합은 스쳐 지나간다. 대신 직선 + 혼합은 구간마다 속도가 일정하고 가속도가 한계를 넘지 않는다. 무엇이 중요한지에 따라 고른다.

영상에서 보기: 36:11 구간이 여러 개일 때 · 39:21 가속도의 크기는 주어진다 · 41:54 첫 구간의 식 · 44:31 경유점을 지나지 않는다 · 45:34 유사 경유점

10.8 5차 다항식, 그리고 다른 곡선

3차 다항식은 가속도를 고를 수 없었다. 그래서 정지 상태에서 출발하는 순간 가속도가 0에서 6Δ/T²로 튄다. 경유점에서도 튄다(그림 10-5). 가속도가 튀면 모터의 힘이 한순간에 바뀌어 팔이 떨린다.

가속도가 바뀌는 빠르기를 저크(jerk)라 한다. 가속도를 미분한 것이라, 가속도가 튀는 순간에는 저크가 무한대다. 자동차가 급출발할 때 목이 젖혀지는 느낌이 큰 저크다.

조건 여섯 개, 계수 여섯 개

양 끝의 위치, 속도, 가속도를 모두 정하면 조건이 여섯 개다. 계수가 여섯 개인 5차 다항식(quintic)을 쓰면 맞출 수 있다.

t = 0의 세 조건에서 a₀ = θ₀(출발 위치), a₁ = θ̇₀(출발 속도), a₂ = θ̈₀/2(출발 가속도의 절반)가 바로 나온다. t = T의 세 조건은 a₃, a₄, a₅에 대한 식 세 개다. 3차식 때와 같이 연립일차방정식을 푼다.

속도와 가속도가 양 끝에서 모두 0이면 답이 깔끔하다. τ = t/T(0에서 1까지 가는 시간 비율)로 쓰면 이렇다.

계수로 쓰면 a₃ = 10Δ/T³, a₄ = −15Δ/T⁴, a₅ = 6Δ/T⁵다.

3차와 5차 비교

같은 Δ = 90°, T = 3 s로 비교한다.

5차식은 처음에 더 조심스럽게 출발한다. 그 대신 가운데서 더 빨리 가야 해서, 최고 속도가 3차식보다 25% 크다. 부드러움을 얻고 속도를 내준 것이다.

잠깐, 생각해 보기. 10τ³ − 15τ⁴ + 6τ⁵이 정말 조건을 맞추는지 τ = 0과 τ = 1을 넣어 확인해 보자.

τ = 0이면 0, τ = 1이면 10 − 15 + 6 = 1이라 위치가 θ₀에서 θ₀ + Δ로 간다. 미분은 30τ²(1 − τ)²이라 양 끝에서 0이다. 한 번 더 미분하면 60τ(1 − τ)(1 − 2τ)라 역시 양 끝에서 0이다.

다항식이 아니어도 된다

조건을 맞출 수만 있으면 sin, cos이나 지수 함수 등 어떤 함수든 쓸 수 있다. 다항식을 많이 쓰는 까닭은 계수를 구하는 식이 일차라서 풀기 쉽기 때문이다. 강의 말로는 "특별히 도전하고 싶은 날"에 다른 함수를 쓰면 된다.

영상에서 보기: 46:35 조건이 많으면 높은 차수 · 47:06 5차 다항식 · 47:37 다른 함수도 된다

10.9 실행할 때: 제어기에 숫자 넘겨주기

지금까지는 식을 만들었다. 실제로 팔을 움직이려면 그 식에서 숫자를 뽑아 제어기에 계속 넘겨주어야 한다.

경로 생성기(path generator)는 정해진 갱신 주기(update rate)마다 지금 시각 t를 식에 넣는다. 그리고 관절마다 각도, 각속도, 각가속도 세 값을 계산해 제어기로 보낸다. 예를 들어 1초에 100번이면 0.01초마다 한 번이다.

제어기가 이 세 값을 어떻게 쓰는지는 13~14강에서 배운다.

• 관절 공간, 3차 다항식: 구간마다 계수 네 개만 저장한다. 시각이 다음 구간으로 넘어가면 다음 계수로 바꾼다.
• 관절 공간, 직선 + 혼합: 매번 지금이 혼합 구간인지 직선 구간인지 보고 맞는 식을 쓴다.
• 작업 공간: 매번 손끝의 위치와 방향을 같은 식으로 계산한다. 그다음 역기구학으로 관절 각도를 구한다. 각속도는 자코비안의 역행렬(θ̇ = J⁻¹ẋ)로, 각가속도는 그것을 한 번 더 미분해서 구한다.

여러 관절을 함께 움직일 때는 시간을 맞춘다. 가장 오래 걸리는 관절에 맞춰 T를 정하고, 다른 관절도 같은 T로 짠다.

알아 두기. 취미용 서보는 "이 각도로 가라"는 명령만 받는다. 목표 각도를 한 번에 보내면 서보가 낼 수 있는 최고 속도로 돌진하고, 도착해서 급히 서며 덜컥거린다. 궤적을 쓰면 20 ms마다 θ(t)를 조금씩 보낸다. 서보는 작은 걸음을 따라가니 3차식이나 5차식의 부드러운 모양대로 움직인다.

영상에서 보기: 49:12 실행할 때의 경로 생성 · 50:15 갱신마다 계수를 바꾼다 · 51:48 작업 공간이면 갱신마다 역기구학 · 52:49 모든 관절의 시간 맞추기

10.10 장애물이 있을 때

지금까지는 경유점이 주어졌다고 했다. 장애물을 피하는 경유점을 어떻게 찾을까? 이것은 운동 계획(motion planning)이라는 큰 분야다.

스탠퍼드에는 Jean-Claude Latombe 교수의 수업이 따로 있었다. 강의는 생각의 줄기만 짚는다.

넓게 보기와 좁게 보기

• 전역 계획(global, gross motion): 빈 공간을 크게 지날 때는 팔 전체의 길을 대충 빠르게 짠다.
• 국소 계획(local, fine motion): 물건이 붐비는 곳에 오면 손끝 근처만 정밀하게 짠다.

실제로는 둘을 섞는다. 6자유도 PUMA 팔이 선반 사이로 물건을 집으러 간다면, 선반 앞까지는 전역 계획으로, 선반 안에서는 국소 계획으로 간다.

설정 공간: 장애물을 부풀려 점 하나로

강의의 예부터 보자. 반지름이 있는 동그란 로봇이 장애물 사이를 지나간다. 장애물을 로봇의 반지름만큼 사방으로 부풀린다.

그러면 로봇의 중심점 하나만 부풀린 장애물을 피하면 된다. 몸집 있는 로봇의 문제가 점 하나의 문제로 바뀐다.

부풀린 장애물을 설정 공간 장애물(C-space obstacle)이라 한다. 2장에서 모양을 정하는 숫자 묶음을 설정 파라미터라 했고, 그 숫자들로 만든 공간이 설정 공간(configuration space)이다. 동그란 로봇은 중심의 (x, y)가, 종이 팔은 관절 각도 (θ₁, θ₂)가 설정 파라미터다.

종이 팔도 같은 생각을 쓸 수 있다. 2장에서 약속한 "판 위에서 점 하나 옮기기"가 이것이다.

책상 위의 컵을 관절 공간에 옮겨 그린다. 자세 (θ₁, θ₂)마다 팔이 컵에 닿는지 보고, 닿으면 그 칸을 칠한다.

해 볼 것:

1. 어깨 θ₁을 60°에 두고 팔꿈치 θ₂를 끝에서 끝까지 돌린다. 링크 1이 컵 위를 지나니 내내 닿는다. 관절 공간에서는 위아래로 꽉 찬 세로 띠다(문제 8).
2. 어깨를 20°에 두고 팔꿈치를 110°~140°로 접는다. 링크 1은 컵을 비켜 가지만, 접힌 링크 2가 컵에 걸린다. 세로 띠의 위쪽 왼편에 붙은 영역이 이런 자세다(아래쪽 오른편은 반대로 꺾은 경우).
3. 관절 공간에서 주황 영역을 피해 가는 길을 그리면, 그 길은 팔 전체가 컵에 부딪히지 않는 길이다.

예를 들어 (20°, 40°)에서 (100°, −40°)로 간다고 하자. 곧장 가면 세로 띠를 넘어야 하는데, 띠가 위아래로 꽉 차 있어 넘을 수 없다.

판의 왼쪽 끝(−180°)과 오른쪽 끝(180°)은 같은 자세다. 그래서 어깨를 시계 방향으로 크게 돌려 왼쪽 끝으로 나가, 오른쪽 끝에서 들어와 100°로 가면 된다. 컵을 등 뒤로 돌아가는 길이다.

관절 공간에서는 장애물을 피하는 문제가 "판 위의 칠한 칸을 피해 점 하나를 옮기는 문제"가 된다. 판을 격자로 나누고 빈 칸끼리 이은 그래프에서 길을 찾으면 된다. 빈 곳을 크게 나누었다가 경계 근처만 잘게 나누는 쿼드트리(quadtree)를 쓰기도 한다.

자유도가 n이면 설정 공간도 n차원이다. 6자유도 팔은 6차원 공간에서 점 하나를 옮기는 문제가 되어, 칸 수가 엄청나게 많아진다. 그래서 운동 계획은 그 자체로 큰 연구 분야다.

잠깐, 생각해 보기. 반지름 3 cm인 동그란 로봇이 있다. 장애물은 한 변이 4 cm인 정사각형이다. 설정 공간 장애물은 어떤 모양일까?

정사각형의 각 변을 바깥으로 3 cm씩 밀어내고, 네 모서리를 반지름 3 cm로 둥글린 모양이다. 가로세로가 4 + 3 + 3 = 10 cm인, 모서리가 둥근 사각형이다. 로봇의 중심이 이 모양 밖에 있으면 로봇은 장애물에 닿지 않는다.

끌어당기고 밀어내기: 인공 퍼텐셜

또 하나의 방법은 1장(1.5절, 그림 1-6)에서 본 것이다. 목표가 손끝을 끌어당기고, 장애물은 가까이 오면 밀어낸다.

이 힘들을 합쳐 따라가면 길이 저절로 생긴다. 이것을 인공 퍼텐셜 방법(artificial potential field)이라 한다.

이 방법은 Khatib 교수가 박사 논문에서 내놓은 것으로, 강의는 당시로서는 혁명적인 일이었다고 소개한다. 미리 경유점을 찾지 않아도 되고, 장애물이 움직여도 바로 반응한다.

대신 끄는 힘과 미는 힘이 서로 비기는 곳에서 멈춰 버릴 수 있다. 이런 곳을 지역 최소점(local minimum)이라 한다.

로봇이 여럿이거나 장애물이 움직이면 더 어려워진다. 다른 로봇을 "움직이는 장애물"로 보고 밀어내는 힘을 준다. 강의 첫머리의 자율 주행 차가 다른 차들 사이에서 하는 일이 이것이다.

영상에서 보기: 53:20 장애물과 운동 계획 · 53:51 전역 계획과 국소 계획 · 55:23 장애물을 로봇 크기만큼 키우기 · 57:57 인공 퍼텐셜

10.11 연습문제

종이에 시간 축을 그리고 곡선을 대충 그려 보면서 푼다. 답은 문제 8개 아래에 있다.

1. 팔꿈치를 30°에서 120°로 2초 동안, 정지에서 정지로 옮긴다. 3차 다항식의 계수 네 개를 구하라. 1초일 때의 각도, 최고 속도, 출발 순간의 가속도는?
2. 서보의 최고 속도는 60°/s, 최고 가속도는 100°/s²다. 90°를 정지에서 정지로 돌린다.
   1. 3차 다항식 하나로 가장 빨리 가려면 몇 초 걸리나? 속도와 가속도 가운데 어느 쪽이 시간을 정하나?
   2. 직선 + 포물선 혼합으로, 최고 속도와 최고 가속도를 그대로 쓰면 몇 초 걸리나?
3. 어깨를 0°, 20°, 60°, 80°, 50°로 1초 간격으로 지나게 한다. 어림 규칙으로 경유점 세 곳의 속도를 정하라. 그리고 20°에서 60°로 가는 구간의 3차 다항식을 구하라.
4. Δ = 60°를 T = 2 s에 직선 + 포물선 혼합으로 간다. 가속도가 80°/s²면 혼합 시간과 직선 구간 속도는? 가속도가 50°/s²면 어떻게 되나?
5. 10.8절의 5차 다항식으로 0°에서 90°까지 3초에 간다. 1.5초일 때의 각도와 속도를 구하라. 3차식과 비교하면 최고 속도는 몇 % 큰가?
6. 종이 팔의 손끝을 (15, 0)에서 (0, 15)로 옮긴다. 팔꿈치는 θ₂ < 0인 답을 쓴다.
   1. 출발과 도착의 관절 각도는?
   2. 관절 공간에서 짜면, 진행 50%일 때 손끝은 어디인가?
   3. 작업 공간에서 직선으로 짜면, 진행 50%일 때 관절 각도는? 팔꿈치는 두 방법에서 각각 얼마나 움직이나?
7. 종이 팔의 손끝을 직선으로 옮기려 한다. 10.3절의 함정 (가)~(다) 가운데 무엇에 걸리는지 말하라.
   1. (14, 1)에서 (−14, 1)로
   2. (14, 2.5)에서 (−14, 2.5)로. 가장 가까운 점에서 θ₂와 det J는?
   3. 어깨 서보가 0°~180°만 돌 때, (15, 6)에서 (−15, 6)으로
8. 그림 10-9의 컵((4, 7) cm, 반지름 2 cm)이 있다. 어깨 θ₁이 어느 범위에 있으면 팔꿈치를 어떻게 돌려도 팔이 컵에 닿나? 그림 10-9의 어떤 모양이 이것인가?

답

답 1. Δ = 90°, T = 2 s다. a₀ = 30, a₁ = 0, a₂ = 3 × 90/4 = 67.5, a₃ = −2 × 90/8 = −22.5다.

1초일 때 30 + 67.5 − 22.5 = 75°다. 딱 가운데 각도다. 최고 속도는 1.5 × 90/2 = 67.5°/s(1초일 때)이고, 출발 순간의 가속도는 6 × 90/4 = 135°/s²다.

답 2.

1. 속도 조건은 1.5Δ/T ≤ 60이라 T ≥ 2.25 s다. 가속도 조건은 6Δ/T² ≤ 100이라 T ≥ √5.4 = 2.32 s다. 둘 다 지키려면 T ≈ 2.32 s이고, 가속도가 시간을 정한다.
2. 60°/s까지 올리는 데 60/100 = 0.6 s가 걸리고, 그동안 ½ × 100 × 0.6² = 18°를 간다(감속도 같다). 남은 90 − 36 = 54°를 60°/s로 0.9 s에 가니 모두 0.6 + 0.9 + 0.6 = 2.1 s다. 한계를 끝까지 쓰니 3차식보다 빠르다.

일반식으로는 T = Δ/v + v/a = 1.5 + 0.6 = 2.1 s다. 10.6절 식으로 확인하면 a t_b(T − t_b) = 100 × 0.6 × 1.5 = 90°로 맞는다.

답 3. 구간 기울기는 20, 40, 20, −30°/s다. 경유점 20°에서는 (20 + 40)/2 = 30°/s, 60°에서는 (40 + 20)/2 = 30°/s다. 80°에서는 20에서 −30으로 부호가 바뀌니 0이다.

20° → 60° 구간은 Δ = 40, θ̇₀ = θ̇_f = 30, T = 1이다. a₂ = 120 − (60 + 30) = 30, a₃ = −80 + 60 = −20이다.

확인: θ(1) = 20 + 30 + 30 − 20 = 60이고, 속도는 θ̇(1) = 30 + 60 − 60 = 30이다.

답 4. 가장 작은 가속도는 4 × 60/2² = 60°/s²다. 80은 그보다 크니 된다. 루트 안은 80² × 4 − 4 × 80 × 60 = 25600 − 19200 = 6400, 루트는 80이다.

t_b = 1 − 80/160 = 0.5 s, 직선 구간 속도는 80 × 0.5 = 40°/s, 직선 구간은 2 − 1 = 1 s다.

가속도가 50°/s²면 60보다 작아 루트 안이 음수다. 2초 안에 60°를 갈 수 없다.

답 5. τ = 0.5에서 10/8 − 15/16 + 6/32 = 0.5라서 각도는 45°다. 속도는 (Δ/T) × 30τ²(1 − τ)² = 30 × 30/16 = 56.25°/s다. 3차식의 최고 속도 45°/s보다 25% 크다.

답 6.

1. 출발 (15, 0)은 θ = (29.5°, −67.6°), 도착 (0, 15)는 θ = (119.5°, −67.6°)다. 두 점 모두 받침에서 15 cm라 θ₂가 같다.
2. 관절 각도의 가운데는 (74.5°, −67.6°)다. θ₂가 그대로라 손끝은 반지름 15 cm, 45° 방향인 (10.61, 10.61)에 있다.
3. 직선의 가운데는 (7.5, 7.5)이고 θ = (90.6°, −108.8°)다. 관절 공간에서는 팔꿈치가 움직이지 않는다. 직선에서는 팔꿈치가 41.2°를 접었다가 다시 41.2°를 편다.

두 손끝 사이는 15 − 10.61 = 4.39 cm다. 그림 10-2(방향 90°, 진행 50%)에서 확인할 수 있다.

답 7.

1. (가)다. 직선이 받침에서 1 cm까지 다가간다. 2 cm 안쪽은 닿지 않으니 가운데를 못 지난다.
2. (나)다. 가장 가까운 점 (0, 2.5)는 닿지만, cos θ₂ = (6.25 − 164)/160 = −0.986이라 |θ₂| = 170.4°다. |det J| = 80 |sin θ₂| = 13.4로 가장 큰 값 80의 6분의 1이라 특이점에 가깝다. 어깨도 짧은 거리에서 크게 돌아야 한다.
3. (다)다. (15, 6)의 두 답은 (45.0°, −52.7°)와 (−1.4°, 52.7°)인데, 둘째는 범위를 조금 벗어난다. (−15, 6)은 (135.0°, 52.7°)로만 되고, 다른 답은 θ₁ = 181.4°라 범위 밖이다. θ₂의 부호가 반대인 두 자세라 직선 위에서 갈아탈 수 없다.

답 8. 팔꿈치와 상관없이 닿으려면 링크 1이 컵에 닿아야 한다. 링크 1은 받침에서 θ₁ 방향으로 뻗은 10 cm 선분이다. 컵 중심은 받침에서 √(16 + 49) = 8.06 cm, 방향 atan2(7, 4) = 60.3°에 있다.

컵 중심에서 그 선까지의 거리 8.06 sin|θ₁ − 60.3°|가 2보다 작으면 닿는다. |θ₁ − 60.3°| < arcsin(2/8.06) = 14.4°이므로 답은 45.9° < θ₁ < 74.6°다. 컵 중심이 8.06 cm로 링크 1 길이 10 cm 안쪽이라, 선분 끝이 모자라지 않는다.

그림 10-9의 관절 공간에서 위아래로 꽉 찬 세로 띠가 이것이다. θ₂를 어떻게 해도 이 띠를 벗어나지 못한다.

파이썬으로 확인

파이썬(numpy)이 있으면 아래를 붙여 넣어 돌려 본다. 주석의 값이 그대로 나온다.

다항식의 계수는 "조건 n개 = 식 n개"인 연립일차방정식을 직접 세워 푼다. 3차식과 5차식을 같은 함수로 구한다.

import numpy as np

np.set_printoptions(precision=2, suppress=True)

def poly_coeffs(T, start, end):
    """끝 조건으로 다항식 계수를 구한다. start, end = (위치, 속도[, 가속도]).
    조건이 4개면 3차식, 6개면 5차식. 연립일차방정식 M a = b를 푼다."""
    n = len(start) + len(end)
    rows, rhs = [], []
    for t, conds in [(0.0, start), (T, end)]:
        for k, value in enumerate(conds):            # k번 미분한 값 = value
            row = [0.0] * n
            for i in range(k, n):
                # t^i를 k번 미분하면 i!/(i-k)! t^(i-k)
                row[i] = np.prod(range(i - k + 1, i + 1)) * t ** (i - k)
            rows.append(row)
            rhs.append(value)
    a = np.linalg.solve(np.array(rows), np.array(rhs))
    return np.round(a, 9) + 0.0                      # -0. 을 0. 으로

def evaluate(a, t):
    """위치, 속도, 가속도"""
    p = np.polynomial.Polynomial(a)
    return np.round([p(t), p.deriv(1)(t), p.deriv(2)(t)], 2) + 0.0

def via_velocities(points, times):
    """어림 규칙: 양쪽 기울기의 평균, 부호가 바뀌면 0. 처음과 끝은 0."""
    v = [0.0]
    for i in range(1, len(points) - 1):
        s1 = (points[i] - points[i - 1]) / (times[i] - times[i - 1])
        s2 = (points[i + 1] - points[i]) / (times[i + 1] - times[i])
        v.append((s1 + s2) / 2 if np.sign(s1) == np.sign(s2) else 0.0)
    return v + [0.0]

def blend_time(d, T, acc):
    """직선 + 포물선 혼합 한 구간: 혼합 시간 t_b와 직선 구간 속도"""
    if acc < 4 * d / T**2:
        return None                                  # 가속도가 모자라 T초 안에 못 간다
    tb = T / 2 - np.sqrt(acc**2 * T**2 - 4 * acc * d) / (2 * acc)
    return float(round(tb, 3)), float(round(acc * tb, 3))

def ik(x, y, sign=-1):
    """종이 팔 역기구학(도). sign=-1이면 θ2 <= 0인 답."""
    c2 = (x * x + y * y - 164) / 160
    t2 = sign * np.arccos(c2)
    t1 = np.arctan2(y, x) - np.arctan2(8 * np.sin(t2), 10 + 8 * np.cos(t2))
    return np.degrees([t1, t2])

# 10.4 예: 어깨 0° -> 90°, 3초, 정지에서 정지
a = poly_coeffs(3, (0, 0), (90, 0))
print(a, evaluate(a, 1.5))                  # [ 0.    0.   30.   -6.67] [45. 45.  0.]
# 문제 1: 30° -> 120°, 2초
a = poly_coeffs(2, (30, 0), (120, 0))
print(a, evaluate(a, 1), evaluate(a, 0)[2])
# [ 30.    0.   67.5 -22.5] [75.  67.5  0. ] 135.0
# 문제 2: 최고 속도 60°/s, 최고 가속도 100°/s²로 90° 돌리기
d, vmax, amax = 90, 60, 100
print(round(1.5 * d / vmax, 3), round(np.sqrt(6 * d / amax), 3))   # 2.25 2.324 (3차식)
print(round(d / vmax + vmax / amax, 3))     # 2.1 (직선 + 혼합)
# 10.5 경유점 예: A 0°, B 30°, C 90°, D 60° (1초 간격)
P = [0, 30, 90, 60]
v = via_velocities(P, [0, 1, 2, 3])
print(v)                                    # [0.0, 45.0, 0.0, 0.0]
for i in range(3):
    s = poly_coeffs(1, (P[i], v[i]), (P[i + 1], v[i + 1]))
    print(s, evaluate(s, 0)[2], evaluate(s, 1)[2])
# [  0.   0.  45. -15.] 90.0 0.0
# [ 30.  45.  90. -75.] 180.0 -270.0
# [ 90.   0. -90.  60.] -180.0 180.0
# 가속도까지 이어지는 경유점 속도: v0 + 4 v1 + v2 = 3 (θ2 - θ0) / h
print(np.linalg.solve([[4, 1], [1, 4]], [3 * (90 - 0), 3 * (60 - 30)]))  # [66.  6.]
# 문제 3
v3 = via_velocities([0, 20, 60, 80, 50], [0, 1, 2, 3, 4])
print(v3, poly_coeffs(1, (20, v3[1]), (60, v3[2])))
# [0.0, 30.0, 30.0, 0.0, 0.0] [ 20.  30.  30. -20.]
# 10.6과 문제 4: 직선 + 포물선 혼합
print(blend_time(90, 3, 72), blend_time(60, 2, 80), blend_time(60, 2, 50))
# (0.5, 36.0) (0.5, 40.0) None
# 10.8과 문제 5: 5차식, 0° -> 90°, 3초, 양 끝의 속도·가속도 0
q = poly_coeffs(3, (0, 0, 0), (90, 0, 0))
print(q, evaluate(q, 1.5))
# [  0.     0.     0.    33.33 -16.67   2.22] [45.   56.25  0.  ]
tt = np.linspace(0, 3, 30001)
vel, acc = evaluate(q, tt)[1], evaluate(q, tt)[2]
print(vel.max(), acc.max(), round(float(tt[acc.argmax()]), 2))    # 56.25 57.74 0.63
# 10.2와 문제 6: 관절 공간 대 직선, (15, 0) -> (0, 15)
qa, qb = ik(15, 0), ik(0, 15)
print(np.round([qa, qb, ik(7.5, 7.5)], 1).tolist())
# [[29.5, -67.6], [119.5, -67.6], [90.6, -108.8]]
qm = np.radians((qa + qb) / 2)              # 관절 각도의 가운데
hand = [10 * np.cos(qm[0]) + 8 * np.cos(qm.sum()),
        10 * np.sin(qm[0]) + 8 * np.sin(qm.sum())]
print(np.round(hand, 2), round(np.hypot(*hand) - np.hypot(7.5, 7.5), 2))   # [10.61 10.61] 4.39
# 문제 7
t2 = np.arccos((2.5**2 - 164) / 160)        # (0, 2.5)에서 |θ2|
print(round(np.degrees(t2), 1), round(80 * np.sin(t2), 1))   # 170.4 13.4
poses = [ik(15, 6, -1), ik(15, 6, +1), ik(-15, 6, +1), ik(-15, 6, -1)]
print(np.round(poses, 1).tolist())
# [[45.0, -52.7], [-1.4, 52.7], [135.0, 52.7], [181.4, -52.7]]
# 문제 8: 링크 1(10 cm)이 컵 (4, 7), 반지름 2에 닿는 θ1 범위
c = np.hypot(4, 7)
mid, half = np.degrees(np.arctan2(7, 4)), np.degrees(np.arcsin(2 / c))
print(round(c, 2), round(mid - half, 1), round(mid + half, 1))   # 8.06 45.9 74.6

10.7절의 여러 구간 직선 + 혼합도 확인할 수 있다. 아래를 위 코드 뒤에 붙인다. Craig 교재 식대로 계산하고, 표의 숫자가 나오는지 본다.

np.set_printoptions(precision=3, suppress=True)

def blend_multi(theta, td, acc_mag):
    """경유점이 여러 개인 직선 + 포물선 혼합 (Craig 식 7.24~7.28).
    돌려주는 것: 혼합 가속도, 혼합 시간, 직선 속도, 직선 시간"""
    th, n = np.asarray(theta, float), len(theta)
    acc, tb, v = np.zeros(n), np.zeros(n), np.zeros(n - 1)
    acc[0] = np.sign(th[1] - th[0]) * acc_mag                      # 첫 구간
    tb[0] = td[0] - np.sqrt(td[0]**2 - 2 * (th[1] - th[0]) / acc[0])
    v[0] = (th[1] - th[0]) / (td[0] - tb[0] / 2)
    acc[-1] = np.sign(th[-2] - th[-1]) * acc_mag                   # 마지막 구간
    tb[-1] = td[-1] - np.sqrt(td[-1]**2 + 2 * (th[-1] - th[-2]) / acc[-1])
    v[-1] = (th[-1] - th[-2]) / (td[-1] - tb[-1] / 2)
    for j in range(1, n - 2):                                      # 가운데 직선
        v[j] = (th[j + 1] - th[j]) / td[j]
    for k in range(1, n - 1):                                      # 가운데 혼합
        acc[k] = np.sign(v[k] - v[k - 1]) * acc_mag
        tb[k] = (v[k] - v[k - 1]) / acc[k]
    half = tb / 2
    half[0], half[-1] = tb[0], tb[-1]                              # 양 끝 혼합은 구간 안에 다 들어간다
    tl = np.array(td) - half[:-1] - half[1:]
    return acc, tb, v, tl

acc, tb, v, tl = blend_multi([0, 30, 90, 60], [1, 1, 1], 150)
print(acc)                          # [ 150.  150. -150.  150.]
print(np.round(tb, 3))              # [0.225 0.175 0.625 0.225]
print(np.round(v, 1))               # [ 33.8  60.  -33.8]
print(np.round(tl, 3))              # [0.687 0.6   0.462]
print(round(tb.sum() + tl.sum(), 6))   # 3.0
# C 근처의 최고 각도: 혼합이 시작될 때의 각도 + 속도가 0이 될 때까지 가는 각도
for a in [150, 400]:
    acc, tb, v, tl = blend_multi([0, 30, 90, 60], [1, 1, 1], a)
    start = 30 + v[1] * (1 - tb[2] / 2)          # 직선 BC는 1초에 30°를 지난다
    print(a, round(start + v[1]**2 / (2 * a), 1))
# 150 83.2
# 400 87.7

스탠퍼드 숙제

스탠퍼드 강의 페이지의 Homework 4에서 3번(정지에서 정지로 가는 3차식 하나, 속도와 가속도 한계로 t_f 정하기)은 이 장으로 풀 수 있다. 문제 2의 (1)이 그 연습이다. 풀이는 같은 페이지의 Solution 4에 있다.

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