13๊ฐ Trajectory generation
์ด์ inverse kinematics ์๊ฐ, ์์ ๊ทธ๋ฆฌ๋ double linked pendulum์ ๊ตฌํํ ๋ฐ ์์ต๋๋ค. Jacobian ์๊ฐ์๋ cartesian ์ขํ๊ณ์์ joint space๋ก์ ๋ณํ์ ํตํด ๋น์ทํ ๊ตฌํ์ ์งํํ ๋ฐ ์์์ง์.
ํ์ง๋ง ์ด๋ ๊ฒ ์์ฑํ trajectory๋ ์ฐ๋ฆฌ๊ฐ ์ํ๋ ์์ ์, ๋ก๋ด์ด ์๋ฒฝํ๊ฒ ํน์ joint angle์ ๊ฐ์ง๋ค๋ ๊ฐ์ ์ ํฌํจํฉ๋๋ค. ๋๋ถ์ด ์ค์ ๋ก๋ด์ ๋ฌผ๋ฆฌ์ ์ธ ํ๊ณ๋ก ์ธํด ์ต๋ ์๋, ์ต๋ ๊ฐ์๋์ ์ ํ์ ๊ฐ์ง์ง์.
๋ฐ๋ผ์ ์ด๋ฒ ์๊ฐ์๋ ๋ณด๋ค ํ์ค์ ์ธ trajectory, joint angle, velocity, effort์ ์๊ฐ์ ๋ํ ๊ฒฝ๋ก๋ฅผ ๊ณ์ฐํด๋ด๋ ๋ฐฉ๋ฒ์ ๋ํด ํจ๊ป ์ดํด๋ณด๊ณ ์ ํฉ๋๋ค.
- ๋ก๋ดํ์ด ์์ง์ด๋ ๊ฒฝ๋ก $q(t)$๋ ๊ณง end effector์ ์ขํ ๋ณํ, ํน์ joint angle์ ์๊ฐ์ ๋ฐ๋ฅธ ๋ณํ๋ผ๊ณ ๋งํ ์ ์์ต๋๋ค. ์ด $q(t)$๋ $x(t), y(t), z(t)$๋ก ํํ๋ ์ ์๊ณ $\theta_1(t), \theta_2(t), \theta_3(t)$๋ก๋ ํํ๋ ์ ์์ ๊ฒ์ ๋๋ค.
- ์ด๊ธฐ ์ํ $q(0)$์์ ๋ชฉ์ ์ง์ $q(f)$์ ๋๋ฌํ๊ธฐ ์ํ ์ต์ ๊ฒฝ๋ก๋ฅผ ์๊ฐํด๋ณด๋ฉด, ๊ฐ๋จํ๊ฒ ๋ ์ง์ ์ ์ง์ ์ผ๋ก ์ฐ๊ฒฐํ๋ ๋ฐฉ์์ ์๊ฐํด๋ณผ ์ ์์ต๋๋ค. ๊ทธ๋ฆฌ๊ณ ์ด๋ ๊ณง 1์ฐจ ํจ์์ ๊ณ์๋ฅผ ๊ตฌํ๋ ๊ฒ๊ณผ ๋์ผํ์ง์.
- 1์ฐจ ํจ์์ ๊ณ์ $a0, a1$ ์ ๋๋ ์์ผ๋ก ํ ์๋ ์์ง๋ง sympy๋ฅผ ํตํด ํ๋ก๊ทธ๋๋ฐ์ผ๋ก ๋์ถํด๋ณด๊ฒ ์ต๋๋ค.
- $t_0 = 0,\,q(t_0) = 10,\,t(t_f) = 1,\, q(t_f) = 20$ ์ ๊ฐ์ ํ์ฌ $q, \dot{q}$ ์ ๊ทธ๋ํ๋ฅผ ๊ทธ๋ ค๋ณด์์ต๋๋ค.
- $\dot{q}$ ์ ๊ทธ๋ํ๋ฅผ ๋ณด๋ฉด, ๊ฐ์๊ธฐ ์๋๋ฅผ 0์์ 10์ผ๋ก ๋์ด๊ณ , ๋ 10์์ 0์ผ๋ก ๋ฎ์ถฐ์ผ ํ๋ ์ง์ ์ด ๋ฐ์ํฉ๋๋ค. ์ด๋ transient๋ผ๊ณ ํ๋ฉฐ, ์ด๋ฌํ ์์ง์์ ๋ชจํฐ์ ํฐ ๋ฌด๋ฆฌ๋ฅผ ์ค๋๋ค.
- ์ด๋ฅผ ํด๊ฒฐํ๊ธฐ ์ํด์ 1๊ณ ๋ฏธ๋ถ ์กฐ๊ฑด์ธ $\dot{q}(t_0) = 0, \, \dot{q}(t_f) = 0$์ด๋ผ๋ ์กฐ๊ฑด์ ์ถ๊ฐํด๋ณด์์ต๋๋ค. ์ข ํฉํด๋ณด๋ฉด ์ด 4๊ฐ์ ์กฐ๊ฑด์ด ๋ง์กฑ๋์ด์ผ ํ๊ณ ์ด๋ ๊ณง ๊ณ์๊ฐ 4๊ฐ์ธ 3์ฐจ ํจ์๋ฅผ ๊ตฌํด๋ด์ผ ํ๋ค๋ ๋ป์ด ๋ฉ๋๋ค.
- ํ๋ ฌ ํํ๋ก ์กฐ๊ฑด์์ ์ ๋ฆฌํ ์ ์์ผ๋ฏ๋ก, ์ด๋ฒ์๋ sympy๋ฅผ ํตํด ํด๋ฅผ ๊ตฌํด๋ณด๊ฒ ์ต๋๋ค. ์ผ์ ์ฝ๋์์ ํ์ฅ๋ง ์ํค๋ฉด ๋๊ธฐ ๋๋ฌธ์ ๊ตฌํ ์์ฒด๋ ๊ฐ๋จํฉ๋๋ค.
- $q, \dot{q}, \ddot{q}$ ์ ๊ทธ๋ํ๋ฅผ ์ดํด๋ด ์๋ค. ๋ฌธ์ ๊ฐ ๋์๋ transient๋ ํด๊ฒฐ๋์์ง๋ง ์ฌ์ ํ $\ddot{q}$ ์์ transient๊ฐ ์กด์ฌํฉ๋๋ค.
- ์ด๋ฅผ ํด๊ฒฐํ๊ธฐ ์ํด ๋ค์ $\ddot{q}$ ์ ๋ํ ์กฐ๊ฑด์ ์ถ๊ฐํด๋ณด๊ฒ ์ต๋๋ค.
- ์ด๋ ๊ฒ ์ด 6๊ฐ์ง ์กฐ๊ฑด์ ๋ชจ๋ ๋ง์กฑํ๋ $q(t)$ ๋ฅผ ์ฐพ๊ธฐ ์ํด์ , 5์ฐจ ๋ฐฉ์ ์์ ๋์ ํด์ผ ํฉ๋๋ค. 5์ฐจ ๋ฐฉ์ ์ 6๊ฐ์ ์กฐ๊ฑด์์ด ์ฌ์ฉ๋๋ฉฐ, ์ด๋ 6 * 6 ์ฌ์ด์ฆ์ ํ๋ ฌ์ ํธ๋ ๋ฌธ์ ๊ฐ ๋๊ธฐ ๋๋ฌธ์ sympy๋ฅผ ์ฌ์ฉํ๋๋ผ๋ ์ค๋ ์๊ฐ์ด ๊ฑธ๋ฆฌ๊ฒ ๋ฉ๋๋ค.
๐ก ์ ์์ ์ ์ข ๋ ํธํ๊ฒ ๊ณต์ํํ ์๋ ์์๊น์? sympy์ diff, subs๋ฅผ ํตํด ๊ตฌํํ๋ ๋ฐฉ๋ฒ์ ์๊ฐํด๋ณด์ธ์!
- ์ต์ข ๊ฒฐ๊ณผ์ ๊ทธ๋ํ๋ฅผ ์ดํด๋ณผ๊น์? $q$์์์ transient, $\dot{q}$์์์ transient, $\ddot{q}$ ์์์ transient๊ฐ ๋ชจ๋ ํด๊ฒฐ๋ ๋ชจ์ต์ด ๋ณด์ ๋๋ค.
๐ ๊ทธ๋ฐ๋ฐโฆ ๋๋ค์ 3๊ณ ๋ฏธ๋ถ์์ transient๊ฐ ๋ณด์ด๋๊ตฐ์โฆ ์ด๋ฅผ ํด๊ฒฐํ๊ธฐ ์ํด์๋ 7์ฐจ ๋ฐฉ์ ์ ํํ์ $q(t)$๋ฅผ ๊ตฌํด์ผ ํฉ๋๋ค. ํ์ง๋ง ๊ณ์ฐ์์ด ๋ณต์กํด์ง ๋ฟ, ์ง๊ธ๊น์ง ๊ฐ์๋ฅผ ์ ๋ฐ๋ผ์ค์ จ๋ค๋ฉด ํด๊ฒฐ ๋ฐฉ๋ฒ์ ๋ํด์๋ ๋ชจ๋ ์ดํดํ์ จ์ผ๋ฆฌ๋ผ ์๊ฐํฉ๋๋ค. ๋ฐ๋ผ์ 5๊ณ, 7๊ณ, 9๊ณ ๋ฏธ๋ถ์ ์ง์นญํ๋ ์ฉ์ด๋ง ์ ๋ฆฌํ๊ณ ๋์ด๊ฐ ๋ณด๊ฒ ์ต๋๋ค.
Trajectory Generation Example
์ง๊ธ๊น์ง ๋ฐฐ์ด ๋ด์ฉ๋ค์ ๋ฐํ์ผ๋ก ์ค์ง์ ์ธ ์์๋ฅผ ์ดํด๋ณด๊ฒ ์ต๋๋ค.
- ์ ์ฒด ๊ฒฝ๋ก๋ 0 ~ 1์ด, 1 ~ 3์ด๋ก ๋๋์ด ์์ผ๋ฉฐ ๊ฐ ๊ฒฝ๋ก๋ง๋ค 4๊ฐ์ฉ์ ์กฐ๊ฑด์ ๊ฐ๋ ์ํฉ์ ๋๋ค. ๋ฐ๋ผ์, 3์ฐจ ํจ์ ๋์์ ๊ณ์ 8๊ฐ๋ฅผ ๊ตฌํด์ผ ํ๋ ๋ฌธ์ ๋ก ์ ์ํ ์ ์์ง์.
- ๊ฐ ์กฐ๊ฑด์ ๋ํ ์์์ ์ ๋ฆฌํ ๋ค 8 * 8 ํฌ๊ธฐ๋ฅผ ๊ฐ๋ ํ๋ ฌ๋ก ์ด๋ฅผ ์ ๋ฆฌํด๋ด ์๋ค.
- ๋ค์์ผ๋ก, sympy๋ฅผ ํตํด ์ด ์์์ ๊ตฌํํ๊ณ , Ax = B ๋ฌธ์ ์ ํด๋ฅผ ๊ตฌํด๋ ๋๋ค.
- Numpy๋ฅผ ํตํด 0 ~ 1์ด, 1 ~ 3์ด์ ๋ํ ๊ฐ๊ฐ์ $q, \dot{q}, \ddot{q}$ ๊ทธ๋ํ๋ฅผ ๊ตฌํด๋ ๋๋ค.
- numpy์ ๊ฒฐ๊ณผ๋ฅผ ๊ทธ๋ํ๋ก ๊ทธ๋ ค๋ณด์์ต๋๋ค. $\dot{q}$์ $\ddot{q}$๋ฅผ ์ดํด๋ณด๋ฉด, t=1์์ transient๊ฐ ๋ฐ์ํ ๊ฒ์ ํ์ธํ ์ ์์ง์!
- t=1 ์์์ transient๋ฅผ ํด๊ฒฐํ๊ธฐ ์ํด ์กฐ๊ฑด์์ ์๋์ ๊ฐ์ด ๋ณ๊ฒฝํด๋ด ์๋ค.
- t=1์์์ ์๋์ ๊ฐ์๋๊ฐ ๋ ๊ฒฝ๋ก์์ ๊ฐ๋ค๋ ์กฐ๊ฑด์ ์ถ๊ฐํด์ค ๊ฒ์ ๋๋ค. ์ด์ ๋ฐ๋ผ ๊ธฐ์กด $\theta_1(t=1), \theta_2(t=1)$ ์๋ ์กฐ๊ฑด ๋์ , ์๋์ ์๋ก์ด ์กฐ๊ฑด์ ์ ์ฉํฉ๋๋ค.
- ์กฐ๊ฑด์์ ๋ณ๊ฒฝ์ ๋ฐ๋ผ ์๋กญ๊ฒ ์ ์๋ Ax = B๋ ๋ค์๊ณผ ๊ฐ์ต๋๋ค.
- numpy๋ฅผ ์ฌ์ฉํ์ฌ ๋ค์ ๊ทธ๋ํ๋ฅผ ๊ทธ๋ ค๋ณด๋ฉด, $\dot{q}$์์์ transient๊ฐ ์ฌ๋ผ์ง ๋ชจ์ต์ ๋ณผ ์ ์์ต๋๋ค! ๋ง์ฝ $\ddot{q}$์์์ transient๋ฅผ ์ ๊ฑฐํ๊ณ ์ถ๋ค๋ฉด, ๊ฒฝ๋ก์์ ์ฐจ์์ ๋์ธ ๋ค์ ์ผ์ ์ํํ๋ ์์ ์ ๋ฐ๋ณต ์ ์ฉํ๋ฉด ๋ ๊ฒ์ ๋๋ค. ์ด๋ ๊ณผ์ ๋ก ์งํํด ๋ณด๊ฒ ์ต๋๋ค.
๊ณผ์ - jerk๋ฅผ ์ฌ์ฉํ์ฌ $\ddot{q}$ ์ ์ต์ ํํ๊ธฐ
์์ ์์์ $\ddot{q}(1)$์ ๋ณด๋ฉด ๊ฐํด์ง๋ ํ ํฌ๊ฐ ๊ฐ์๊ธฐ ๋ณํํ๋ ๊ฒ์ ์ ์ ์์ต๋๋ค. ์ด๋ ๊ณง jerk์ transient๋ฅผ ์๋ฏธํ์ง์. ๋ฐ๋ผ์ 3๊ณ ๋ฏธ๋ถ์ ๋ํ ์กฐ๊ฑด์ ์ถ๊ฐํ์ฌ ์๋์ ๊ฐ์ด ์ค๋ฌด์คํ q_dotdot์ ๊ตฌํํด๋ณด๋ ๊ณผ์ ๋ฅผ ์ ์๋๋ฆฝ๋๋ค.
jeck ์กฐ๊ฑด์ด ์ถ๊ฐ๋๋ฏ๋ก q(t)๋ 4์ฐจ ๋ฐฉ์ ์์ด ๋ ๊ฒ์ด๋ฉฐ, ์ด 9๊ฐ์ ์กฐ๊ฑด์์ ์ฌ์ฉํ๊ฒ ๋ ๊ฒ์ ๋๋ค.
๊ณผ์ 2 - Velocity Upper Bound
- ์์ trajectory์ velocity๋ฅผ ์ดํด๋ณด๋ฉด 0.5๋ฅผ ๋์ด ํผํฌ๋ฅผ ์ฐ์ ๋ค ๋ค์ ๊ฐ์ํ๋ ๋ชจ์ต์ ๋ณผ ์ ์์ต๋๋ค. ํ์ง๋ง ์ค์ ๋ชจํฐ๋ ์ต๋, ์ต์ ์๋๋ฅผ ๊ฐ๊ธฐ ๋๋ฌธ์ ์ด๋ฌํ ๋์์ด ๋ถ๊ฐํ ์ ์์ต๋๋ค.
- ์ด๋ฅผ ํด๊ฒฐํ๊ธฐ ์ํด ์ง๋ ์๊ฐ ํ์ตํ
opt.minimize trajectory
๋ฅผ ์ฌ์ฉํ์ฌ ์ต๋ ์๋ ์ ํ์ ๋ง์กฑํ๋ ์ต์ ๊ฒฝ๋ก๋ฅผ ๊ตฌํด๋ด ์๋ค!