5.2 Interpolation

---

5.2.1 Multidimensional Motion

로봇들은 여러 축에 대해서 운동한다. 이에 대해 smooth function을 만들려면?

$s$를 smooth function이라고 하자. Polynomial이든 trapezoidal이든 상관없다. 이 $s$를 linear interpolation을 이용해 real joint(Cartesian variable)로 mapping하려고 한다.

$\textbf{x}$를 vector라고 하면,

\[\textbf{x}(s) = (1-s)\textbf{x}_i + s \textbf{x}_f \qquad s \in [0,1]\]

같은 원리로 rotation도 interpolation 해 보자. $\textbf{R}$을 rotation matrix라고 할 때,

\[\textbf{R}(s) = (1-s)\textbf{R}_i + s \textbf{R}_f \qquad s \in [0,1]\]

잠깐, rotation matrix는 orthogonal인데, 두 orthogonal matrix를 더한다고 해서 orthogonal이 될 리가 없다. 다른 방식이 필요하다.

3-angle representation이라면 가능하다. Euler angle 또는 RPY rotation $\boldsymbol{\Gamma}$에 대해서,

\[\boldsymbol{\Gamma}(s) = (1-s)\boldsymbol{\Gamma}_i + s \boldsymbol{\Gamma}_f \qquad s \in [0,1]\]

Quaternion은? 이 경우는 살짝 복잡하다.

\[q(s) = \frac{\sin((1-s)\varphi)}{\sin\varphi}q_1 + \frac{\sin(s\varphi)}{\sin \varphi}q_2 \qquad s \in [0,1] \\ \cos \varphi = q_1 \cdot q_2\]

여기서 $q_1, q_2$는 unit quaternion이고, $\varphi$는 $q_1$과 $q_2$ 사이의 angular distance의 절반.

---

5.2.2 Lerp

Linear interpolation을 Lerp라고 한다.

\[\begin{align*} \textbf{v}(s) &= (1-s)\textbf{v}_i + s \textbf{v}_f \qquad s \in [0,1] \\ \textbf{v}(s) &= \textbf{v}_i + s\textbf{v}_f \end{align*}\]

---

5.2.3 NLerp

NLerp는 normalized linear interpolation을 뜻한다. Lerp의 결과를 normalize하여 단위 길이를 유지시킨다.

\[\begin{align*} \textbf{v}(s) &= (1-s)\textbf{v}_i + s \textbf{v}_f \qquad s \in [0,1] \\ \textbf{v}(s)^{\ast} &= \frac{\textbf{v}(s)}{\Vert \textbf{v}(s) \Vert} \end{align*}\]

---

5.2.4 SLerp

SLerp는 spherical linear interpolation의 약자로, 3D rotation의 animation을 목적으로 개발된 quaternion interpolation이다.

\[\begin{align*} \textbf{v}(s) &= n\textbf{v}_i + m \textbf{v}_f \qquad s \in [0,1] \\ \textbf{v}(s) &= \frac{\sin((1-s)\varphi)}{\sin\varphi}\textbf{v}_i + \frac{\sin(s\varphi)}{\sin \varphi}\textbf{v}_f \end{align*}\]

---

5.2.5 Interpolating Pose

이제 pose를 interpolate해 보자. Pose는 traslation과 rotation으로 이루어져 있으므로 각각 성분들을 interpolate 시켜야 한다.

각 좌표계 $\boldsymbol{\xi}_i$, $\boldsymbol{\xi}_f$마다 position과 quaternion이 존재하므로 각각에 interpolation을 취하면 된다.

---

5.2.6 Joint Space Interpolation

Initial pose와 final pose가 주어졌을 때 $\boldsymbol{\xi}_i$에서 $\boldsymbol{\xi}_f$로의 intermediate joint angle들을 찾고자 한다.

각 pose에 따른 joint angle을 결정,

\[\boldsymbol{\theta}_i = \textbf{K}^{-1}(\xi_i) \\ \boldsymbol{\theta}_f = \textbf{K}^{-1}(\xi_f) \\\]

이전 논의에 따라 joint space 내에서의 smooth function을 만들 수 있다.

---

5.2.7 Cartesian Space Interpolation

Initial pose와 final pose가 주어졌을 때 Cartesian space 내에서 intermediate joint angle들을 찾고자 한다면 각 intermediate point를 먼저 결정.

\[\boldsymbol{\xi}_s = f(\boldsymbol{\xi}_i, \boldsymbol{\xi}_f, s) \qquad s \in [0, 1]\]

그 다음에 inverse kinematics.

\[\boldsymbol{\theta}_s = \textbf{K}^{-1}(\xi_s)\]

각각의 step마다 pose를 interpolate하고 inverse kinematics를 계산.