Trajectory Interpolator
========================


Quick Introduction
~~~~~~~~~~~~~~~~~~~~~

=========================================
Why do we need trajectory interpolation?
=========================================

For a majority of policies in robot learning research, we care about the action output of a policy $a_{t}=\pi(a_{t}|\cdot)$. Such a policy is typically categorized as a high-level decision making. It happens at a *low-frequency*, which is different from the *high-frequency* control $u$ a robot manipulator requires. 

Therefore, in most cases, what actually happens is that the robot controllers will expand the high-level action commands into a sequence of fine-grained actions. More concretely, for an action $a_{t}$, we computes $T$ high-frequency control commands $\{u^{(t)}_{i}\}_{i=1}^{T}$ using trajectory interpolation. Interpolated control commands $\{u^{(t)}_{i}\}$ are converted to torque commands $\{\tau_{i}\}$ that are sent to the robot via robot's control api. For simplicity, we assume $a\in\mathcal{A}$, where $\mathcal{A}$ is the action space of joint positions and cartesian positions (Note that it can also contains velocities, but we do not consider in this codebase at the moment).

==============================================
How is a trajectory interpolation implemented?
==============================================

In our codebase, our default setting is 20Hz for $a_{t}$ and 500Hz for $u^{(t)}_{i}$. You might notice that Franka Emika Panda requires 1000Hz control loop. We fulfill this requirement by repeating each $u^{(t)}_{i}$ twice to avoid jittering motion. In summary, each action $a_{t}$ action from the high-level policy is converted to 25 $u^{(t)}_{i}$ commands by default. The following animation visualizes how trajectory interpolations work.

.. raw:: html

    <video autoplay muted loop src="../_images/traj_interpolation_explanation.mp4" width="100%">
    </video>   

.. .. image:: ../_images/traj_interpolation_explanation.mp4
..    :width: 100%

A most straightforward way to compute interpolation commands is to interpolate between the current robot state and the given action command. However, this introduces jitterness in actual control. The reason is that the previous commands are computed based on $a_{t-1}$, not the current robot state $x_{t}$. There are always errors between $x_{t}$ and $a_{t-1}$. Therefore, if the interpolation is between $x_{t}$ and $a_{t}$, it introduces nonsmoothness in the high-frequency control commands $\{u^{(t)}_{i}\}$, and you would hear the noise when robots move. To ensure smoothness in the the commands, we interpolate between $a_{t-1}$ and $a_{t}$.


Trajectory Interpolator Type
~~~~~~~~~~~~~~~~~~~~~~~~~~~~

In this codebase, we majorly implement two types of interpolators: linear interpolators and min jerk interpolators. One interpolates linearly between $a_{t-1}$ and $a_{t}$, and the other interpolates to regularize the jerkiness between each point. Both types are implemented for joint positions and cartesian poses.

===================
Linear Interpolator
===================

Linear interpolator is very simple and straightforward. It aims to computes control commands $\{u^{(t)}_{i}\}$ that evenly divide between $a_{t-1}$ and $a_{t}$:

$$\hat{t} = \min(\max(\frac{i}{\alpha T}, 0), 1)$$
$$u^{(t)}_{i} = (a_{t} - a_{t-1})\hat{t} + a_{t-1}$$

where $\alpha \in [0.1, 1.0] $ which tunes the time fraction of trajectory interpolation within each $T$ steps. For example, if $alpha=0.33$, that means that the control command becomes $a_{t}$ within $\frac{1}{3}$ of an interpolation interval, and remain the same for the rest of $\frac{2}{3}$ within the $T$ step interpolation interval.

.. note::
    You can set $\alpha$ smaller than $0.1$, but that would cause rapid motion that are not desired. Therefore, we clip the value with a lower bound $0.1$.

=====================
Min Jerk Interpolator
=====================

The way to regularize the jerkiness between each control commmand is to make sure that their high-order derivaives are continuous and smooth. This is the basic idea behind min jerk interpolators. The following formula shows how the min jerk interpolators are implemented in ``Deoxys``.

$$t' = \min(\max(\frac{i}{\alpha T}, 0), 1)$$
$$\hat{t} = 10{t'}^{3} - 15{t'}^{4} + 6{t'}^{5}$$
$$u^{(t)}_{i} = (a_{t} - a_{t-1})\hat{t} + a_{t-1}$$

where $\alpha \in [0.1, 1.0] $, which is defined in the same way as in linear interpolator case. In the following figure, we show a plot of $\hat{t}$ when  $\alpha T=4$.


=====================
Cosine Interpolator
=====================

$$\hat{t} = \frac{1}{2}(1 - \cos{\frac{\pi t}{\alpha T}})$$
$$u^{(t)}_{i} = (a_{t} - a_{t-1})\hat{t} + a_{t-1}$$

Here we refer to $\hat{t}$ as the cosine profile. In the following figure, we show an example of cosine profile with $\alpha T=4$.


==========================================
Comparing Different Interpolators
==========================================

Here we compare the three interpolators we have implemented in this codebase. The following figure shows the comparison of the three interpolators with $\alpha T=4$, and the y axis refers to the profile value $\hat{t}$.

.. raw:: html

    <div id="combined-plotly-div"></div>

    <div id="derivative-combined-plotly-div"></div>

    <script>

        var dt = 0.004;
        var max_t = 4;

        // Generate Linear Interpolation Curve
        var x_values_linear = [];
        var y_values_linear = [];
        var dy_values_linear = [];

        for (var i = 0; i <= 1000; i++) {
            var t = dt * i;
            var x = t / max_t;
            x_values_linear.push(t);
            y_values_linear.push(x);
            dy_values_linear.push(1);
        }

        var traceLinear = {
            x: x_values_linear,
            y: y_values_linear,
            mode: 'lines',
            type: 'scatter',
            name: 'Linear Curve'
        };

        var traceLinear_derivative = {
            x: x_values_linear,
            y: dy_values_linear,
            mode: 'lines',
            type: 'scatter',
            name: 'Linear Curve'
        };

        // Generate Min Jerk Curve
        var x_values_jerk = [];
        var y_values_jerk = [];
        var dy_values_jerk = [];
        for (var i = 0; i <= 1000; i++) {
            var t = dt * i;
            var x = t / max_t;
            x_values_jerk.push(t);
            y_values_jerk.push(10 * Math.pow(x, 3) - 15 * Math.pow(x, 4) + 6 * Math.pow(x, 5));
            dy_values_jerk.push(30 * Math.pow(x, 2) - 60 * Math.pow(x, 3) + 30 * Math.pow(x, 4));
        }

        var traceJerk = {
            x: x_values_jerk,
            y: y_values_jerk,
            mode: 'lines',
            type: 'scatter',
            name: 'Min Jerk Curve'
        };

        var traceJerk_derivative = {
            x: x_values_jerk,
            y: dy_values_jerk,
            mode: 'lines',
            type: 'scatter',
            name: 'Min Jerk Curve'
        };
        
        // Generate Cosine Curve
        var x_values_cosine = [];
        var y_values_cosine = [];
        var dy_values_cosine = [];
        for (var i = 0; i <= 1000; i++) {
            var t = dt * i;
            var x = Math.PI * t / max_t;
            x_values_cosine.push(t);
            y_values_cosine.push((1 - Math.cos(x)) * 0.5);
            dy_values_cosine.push(Math.sin(x) * 0.5 * Math.PI / max_t);
        }

        var traceCosine = {
            x: x_values_cosine,
            y: y_values_cosine,
            mode: 'lines',
            type: 'scatter',
            name: 'Cosine Curve'
        };
        
        var traceCosine_derivative = {
            x: x_values_cosine,
            y: y_values_cosine,
            mode: 'lines',
            type: 'scatter',
            name: 'Cosine Curve'
        };

        var data = [traceLinear, traceJerk, traceCosine];

        var layout = {
            title: 'Comparison of Different Interpolation Methods',
            xaxis: {
                title: 'Time',
                showarrow: true,
                arrowhead: 4,
                arrowcolor: '#333'
            },
            yaxis: {
                title: 'Derivative Value'
            }
        };

        Plotly.newPlot('combined-plotly-div', data, layout);

        var derivative_data = [traceLinear_derivative, traceJerk_derivative, traceCosine_derivative];

        var layout_derivative = {
            title: 'Derivatives of Different Interpolation Methods',
            xaxis: {
                title: 'Time',
                showarrow: true,
                arrowhead: 4,
                arrowcolor: '#333'
            },
            yaxis: {
                title: 'Profile Value'
            }
        };

        Plotly.newPlot('derivative-combined-plotly-div', derivative_data, layout_derivative);

    </script>