~~NOTOC~~

===== Homework 06 - Rotation Interpolation =====


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:
  - 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.
  - Smooth movements help maintain precision in the end-effector's path, which is critical for tasks like assembly, welding, or surgery.
  - 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 [[https://en.wikipedia.org/wiki/Interpolation | 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.

==== 1. Linear interpolation ====

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$.

{{courses:pkr:labs:hw06-lin-inter.png?500|}}

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]. $$
==== 2. Interpolation on the unit sphere ====

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 [[https://en.wikipedia.org/wiki/Great_circle| 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.

{{courses:pkr:labs:hw06-geodesic.png?500|}}

=== 2.1. Normalized linear interpolation (Nlerp) ===

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.$

=== 2.2. Spherical linear interpolation (Slerp) ===

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].$$

=== 2.3 Comparison of velocities ===

{{courses:pkr:labs:hw06-velocities.png?800|}}

==== 3. Rotation Interpolation ====

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).

=== 3.1. Interpolation using angle-axis 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.

{{courses:pkr:labs:hw06-interp-aa.png?800|}}

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)$.

=== 3.2. Interpolation using quaternion representation ===

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.

{{courses:pkr:labs:hw06-interp-q.png?500|}}

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)$.


==== 4. Task ====

**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'':
  - ''R0'': 3x3 rotation matrix of type ''np.ndarray'' at time $t = 0$.
  - ''Ri'': 3x3 rotation matrix of type ''np.ndarray'' at time $t = 0.5$.
  - ''R1'': 3x3 rotation matrix of type ''np.ndarray'' at time $t = 1$.
  - ''t'': float number from the interval $[0,1]$.
  - **Return value**: interpolated 3x3 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'':
  - ''R0'': 3x3 rotation matrix of type ''np.ndarray'' at time $t = 0$.
  - ''Ri'': 3x3 rotation matrix of type ''np.ndarray'' at time $t = 0.5$.
  - ''R1'': 3x3 rotation matrix of type ''np.ndarray'' at time $t = 1$.
  - ''t'': float number from the interval $[0,1]$.
  - **Return value**: interpolated 3x3 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'':
  - ''R0'': 3x3 rotation matrix of type ''np.ndarray'' at time $t = 0$.
  - ''Ri'': 3x3 rotation matrix of type ''np.ndarray'' at time $t = 0.5$.
  - ''R1'': 3x3 rotation matrix of type ''np.ndarray'' at time $t = 1$.
  - ''t'': float number from the interval $[0,1]$.
  - **Return value**: interpolated 3x3 rotation matrix of type ''np.ndarray'' at given time ''t''.

=== Report ===
Follow the [[https://github.com/kzorina/view_rotation_interpolation | 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 ===

Upload a zip archive ''hw06.zip'' (via the [[https://cw.felk.cvut.cz/upload/|course ware]]) containing:
  - ''hw06.py'' - python script containing the implemented functions ''interp_aa'', ''nlerp'', ''slerp''
  - ''hw06.pdf'' - report file containing visualization of the interpolation functions and discussion

<note>
All the files must be contained in the root of ''hw06.zip''.
</note>