In robotics, when performing tasks with a robotic arm, it is usually desired to move the end-effector of the robotic arm through a sequence of poses $(\mathbf{R}_1, \mathbf{t}_1), \dots, (\mathbf{R}_k, \mathbf{t}_k) \in \mathrm{SO}(3) \times \mathbb{R}^3$. Moreover, we want the motion from $(\mathbf{R}_j, \mathbf{t}_j)$ to $(\mathbf{R}_{j+1}, \mathbf{t}_{j+1})$ to be smooth. There are various reasons for that:

1. We would like to avoid abrupt changes in position and orientation since they can cause excessive strain on the robotic arm's joints, motors, and actuators. Smooth motion reduces wear and tear, extending the lifespan of the robotic hardware.
2. Smooth movements help maintain precision in the end-effector's path, which is critical for tasks like assembly, welding, or surgery.
3. Necessity to ensure safety: in collaborative robotics, where robots work alongside humans, sudden jerks or rapid changes in motion can pose safety risks.

In order to achieve a smooth motion we can apply various interpolation techniques. The goal of this task is to learn the basics of rotation interpolation (it is a little more involved than interpolation of translations).

Consider a subset $X \subseteq \mathbb{R}^n$ and two elements $\mathbf{v}_0, \mathbf{v}_1 \in X$. An interpolation path in $X$ from $\mathbf{v}_0$ to $\mathbf{v}_1$ is a continuous map $\mathbf{f}: [0,1] \rightarrow X$ (from the unit interval to $X$) such that $$\mathbf{f}(0) = \mathbf{v}_0, \quad \mathbf{f}(1) = \mathbf{v}_1.$$ Notice that the space $\mathbb{R}^{m\times n}$ of $m \times n$ matrices can be identified with $\mathbb{R}^{mn}$. Hence the notion of the interpolation path is also applicable to the pair of matrices.

As the name suggests, this interpolation technique is based on linearly connecting the two vectors $\mathbf{v}_0$ and $\mathbf{v}_1$ from $\mathbb{R}^n$ (see the image below). An interpolation path $\boldsymbol{\alpha}: [0,1] \rightarrow \mathbb{R}^n$ that linearly connects $\mathbf{v}_0$ and $\mathbf{v}_1$ can be written as

$$ \boldsymbol{\alpha}(t) = (1-t)\mathbf{v}_0 + t\mathbf{v}_1 $$

By direct substitution we can verify that $\boldsymbol{\alpha}(0) = \mathbf{v}_0$ and $\boldsymbol{\alpha}(1) = \mathbf{v}_1$.

This interpolation path follows from $\mathbf{v}_0$ to $\mathbf{v}_1$ with constant velocity: the velocity vector is $$ \frac{\mathrm{d}\boldsymbol{\alpha}}{\mathrm{d}t}(t) = \mathbf{v}_1 - \mathbf{v}_0 \quad \forall t \in [0,1] $$ and the velocity is $$ \left\lVert \frac{\mathrm{d}\boldsymbol{\alpha}}{\mathrm{d}t}(t) \right\rVert = \lVert \mathbf{v}_1 - \mathbf{v}_0 \rVert \quad \forall t \in [0,1]. $$

As the name suggests, this interpolation technique is intended for interpolating between vectors from the unit sphere while staying on the sphere. In other words, for

$$S^n = \{\mathbf{v} \in \mathbb{R}^{n+1} \;|\; ||\mathbf{v}|| = 1\}$$

and $\mathbf{v}_0, \mathbf{v}_1 \in S^n$ we are looking for the interpolation path from $\mathbf{v}_0$ to $\mathbf{v}_1$. A path whose length is the smallest is given by the arc on the great circle defined by $\mathbf{v}_0$ and $\mathbf{v}_1$. This great circle is the intersection of the sphere with the plane $\sigma$ spanned by $\mathbf{v}_0$ and $\mathbf{v}_1$ (see the image below). We will describe two different interpolation paths $\boldsymbol{\beta}: [0,1] \rightarrow S^n$ and $\boldsymbol{\gamma}: [0,1] \rightarrow S^n$ which follow this arc with different velocity functions.

Having two vectors $\mathbf{v}_0, \mathbf{v}_1 \in S^n$ such that $\mathbf{v}_0 \neq -\mathbf{v}_1$ we construct the interpolation path $\boldsymbol{\beta}: [0,1] \rightarrow S^n$ from $\mathbf{v}_0$ to $\mathbf{v}_1$ by

$$\boldsymbol{\beta}(t) = \frac{1}{||\boldsymbol{\alpha}(t)||}\cdot\boldsymbol{\alpha}(t)$$

where $\boldsymbol{\alpha}$ is the interpolation path described in Section 1. The condition $\mathbf{v}_0 \neq -\mathbf{v}_1$ insures that $||\boldsymbol{\alpha}(t)|| \neq 0$ for all $t \in [0,1]$ (notice that for $\mathbf{v}_0 = -\mathbf{v}_1$ we have $\boldsymbol{\alpha}(0.5) = 0.5\cdot(\mathbf{v}_0 + \mathbf{v}_1) = \mathbf{0}$). This path goes with variable velocity $\Big\lVert\frac{\mathrm{d}\boldsymbol{\beta}}{\mathrm{d}t}(t)\Big\rVert$ along the arc: it is increasing as $t$ goes from $0$ to $\frac{1}{2}$ and decreasing as $t$ goes from $\frac{1}{2}$ to $1.$

Another way to interpolate from $\mathbf{v}_0$ to $\mathbf{v}_1$ in $S^n$ such that $\mathbf{v}_0 \neq \pm \mathbf{v}_1$ while staying on $S^n$ is to define the interpolation path $\boldsymbol{\gamma}: [0,1] \rightarrow S^n$ by:

$$\boldsymbol{\gamma}(t) = \frac{\sin((1-t)\Omega)}{\sin(\Omega)}\mathbf{v}_0 + \frac{\sin(t\Omega)}{\sin(\Omega)}\mathbf{v}_1, \quad \Omega = \angle(\mathbf{v}_0, \mathbf{v}_1) = \arccos(\mathbf{v}_0^\top\mathbf{v}_1)$$

The condition $\mathbf{v}_0 \neq \pm\mathbf{v}_1$ insures that $\sin(\Omega) \neq 0$. The advantage of such a definition is that the velocity of this interpolation path is constant along the whole arc and this velocity is equal

$$\Bigg\lVert\frac{\mathrm{d}\boldsymbol{\gamma}}{\mathrm{d}t}(t)\Bigg\rVert = \Omega \quad \forall t \in [0,1].$$

If we want to interpolate linearly between two rotations given by matrices $\mathbf{R}_0$ and $\mathbf{R}_1$ in $\mathbb{R}^{3\times 3}$, then the matrices of the form $$ \mathbf{R}(t) = (1-t)\mathbf{R}_0 + t\mathbf{R}_1, \quad t \in [0,1] $$ will not be rotations in general in the sense that the norms $$||\mathbf{R}(t)^\top\mathbf{R}(t) - \mathbf{I}|| \quad \text{and} \quad |\mathrm{det}(\mathbf{R}(t)) - 1|$$ will be far from zero for a random $t \in [0,1]$. We can deal with this problem by appropriately parameterizing the rotations (i.e. by using angle-axis or quaternion representation).

Remember that for a given rotation matrix $\mathbf{R}$ (which is not the identity and not the rotation by $180^\circ$) there exists two angle-axis representations $(\theta, \mathbf{v}), (-\theta, -\mathbf{v}) \in (-\pi, \pi] \times S^2$. If it is a rotation by $\pi$, then the representatives are $(\pi, \mathbf{v})$ and $(\pi, -\mathbf{v})$. We will further investigate the case when $\theta \neq \pi$.

If we are given two rotation matrices $\mathbf{R}_0$ and $\mathbf{R}_1$ between which we need to interpolate, then we obtain 4 angle-axis representations: $(\theta_0, \mathbf{v}_0), (-\theta_0, -\mathbf{v}_0)$ that represent $\mathbf{R}_0$ and $(\theta_1, \mathbf{v}_1), (-\theta_1, -\mathbf{v}_1)$ that represent $\mathbf{R}_1$. Hence there are 4 possible choices to interpolate in the angle-axis space of representations: considering either $(\theta_0, \mathbf{v}_0)$ or $(-\theta_0, -\mathbf{v}_0)$ for the starting point and $(\theta_1, \mathbf{v}_1)$ or $(-\theta_1, -\mathbf{v}_1)$ for the ending point (see the image below). We use $\alpha$ from Section 1 to interpolate between the angles and $\boldsymbol{\beta}$ from Section 2.1 to interpolate between the axes.

These 4 cases result in 4 different interpolation paths $\boldsymbol{\delta}_1$ (red), $\boldsymbol{\delta}_2$ (blue), $\boldsymbol{\delta}_3$ (green) and $\boldsymbol{\delta}_4$ (magenta), where

$$ \boldsymbol{\delta}_j(t) = (\alpha_j(t), \boldsymbol{\beta}_j(t)) \quad \forall j \in \{1,\dots,4\}, \forall t \in [0,1]. $$

For example, $\boldsymbol{\delta}_2(t) = (\alpha_2(t), \boldsymbol{\beta}_2(t))$, where $\alpha_2$ interpolates from $-\theta_0$ to $\theta_1$ and $\boldsymbol{\beta}_2$ from $-\mathbf{v}_0$ to $\mathbf{v}_1$. Notice that the interpolation paths $\boldsymbol{\delta}_1$ and $\boldsymbol{\delta}_3$ generate equal paths in the space of rotation matrices $\mathrm{SO}(3)$. The same it is with $\boldsymbol{\delta}_2$ and $\boldsymbol{\delta}_4$. Hence we will further discuss only the paths $\boldsymbol{\delta}_1$ and $\boldsymbol{\delta}_2$. Another observation which we can make is that $\boldsymbol{\delta}_1$ and $\boldsymbol{\delta}_2$ generate disjoint trajectories in $\mathrm{SO}(3)$ (if we omit the starting and ending rotation). This is because on the red and blue arcs in $S^2$ there are no two axes which are equal up to sign. Hence, if we want to choose between $\boldsymbol{\delta}_1$ and $\boldsymbol{\delta}_2$, it is enough to specify a rotation matrix $\mathbf{R}_i$ somewhere in the middle of the trajectories that $\boldsymbol{\delta}_1$ and $\boldsymbol{\delta}_2$ generate in $\mathrm{SO}(3)$.

Remember that for a given rotation matrix $\mathbf{R}$ there exists two quaternion representations $\mathbf{q}, -\mathbf{q} \in S^3$. Thus, the situation is exactly the same as in Section 3.1: there are 4 different possibilities to interpolate in the space of quaternions. In the image below, the interpolation paths $\boldsymbol{\delta}_j$ are constructed according to either Section 2.1 or Section 2.2.

Exactly as in Section 3.1: we can focus on studying $\boldsymbol{\delta}_1$ and $\boldsymbol{\delta}_2$ and in order to choose between these two paths it is enough to specify a rotation matrix $\mathbf{R}_i$ in the middle of the trajectories that $\boldsymbol{\delta}_1$ and $\boldsymbol{\delta}_2$ generate in $\mathrm{SO}(3)$.

a. Implement function interp_aa(R0, Ri, R1, t) that interpolates between two rotations using angle-axis representation according to Section 3.1.

I/O specifications for interp_aa:

1. R0: 3×3 rotation matrix of type np.ndarray at time $t = 0$.
2. Ri: 3×3 rotation matrix of type np.ndarray at time $t = 0.5$.
3. R1: 3×3 rotation matrix of type np.ndarray at time $t = 1$.
4. t: float number from the interval $[0,1]$.
5. Return value: interpolated 3×3 rotation matrix of type np.ndarray at given time t.

b. Implement function nlerp(R0, Ri, R1, t) that interpolates between two rotations using quaternion representation according to Sections 3.2 and 2.1.

I/O specifications for nlerp:

1. R0: 3×3 rotation matrix of type np.ndarray at time $t = 0$.
2. Ri: 3×3 rotation matrix of type np.ndarray at time $t = 0.5$.
3. R1: 3×3 rotation matrix of type np.ndarray at time $t = 1$.
4. t: float number from the interval $[0,1]$.
5. Return value: interpolated 3×3 rotation matrix of type np.ndarray at given time t.

c. Implement function slerp(R0, Ri, R1, t) that interpolates between two rotations using quaternion representation according to Sections 3.2 and 2.2.

I/O specifications for slerp:

1. R0: 3×3 rotation matrix of type np.ndarray at time $t = 0$.
2. Ri: 3×3 rotation matrix of type np.ndarray at time $t = 0.5$.
3. R1: 3×3 rotation matrix of type np.ndarray at time $t = 1$.
4. t: float number from the interval $[0,1]$.
5. Return value: interpolated 3×3 rotation matrix of type np.ndarray at given time t.

Follow the repo to study visually the behavior of different interpolation functions that you've implemented. Please include screenshots of all three tabs you will create with the main function. Also, include the discussion: are the functions behaving the same? If not, what is the difference?

Upload a zip archive hw06.zip (via the course ware) containing:

1. hw06.py - python script containing the implemented functions interp_aa, nlerp, slerp
2. hw06.pdf - report file containing visualization of the interpolation functions and discussion