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๋ฅผ ์‚ฌ์šฉํ•˜์—ฌ ์ตœ๋Œ€ ์†๋„ ์ œํ•œ์„ ๋งŒ์กฑํ•˜๋Š” ์ตœ์  ๊ฒฝ๋กœ๋ฅผ ๊ตฌํ•ด๋ด…์‹œ๋‹ค!

Complete and Continue