Road Runner Lite From Scratch

This tutorial walks through the creation of a lite Road Runner. The goal is to cover all of the math and code for a simple version. Hopefully this clarifies some of the magic inside the library and provides a basis for further experimentation. For a more complete exposition of what Road Runner offers, see the tour portion of the docs. All you need to begin is familiarity with single-variable calculus, vectors, and parametric curves. If you're just lacking the last prerequisite, we recommend working through this unit on Khan Academy.

For now, the tutorial is written in Python instead of Kotlin. The code intentionally avoids advanced features when simple ones will do. It aims to be direct and comprehensible at the cost of being idiomatic. We begin with some standard imports.

In [ ]:
import numpy as np
import matplotlib.pyplot as plt

vectorize_tail = lambda f: np.vectorize(f, excluded={0})

Trajectories

Let's focus first on moving smoothly in a single dimension. Say we have a robot with position $x$ that we want to move from rest at $x_0$ to rest at $x_1$ (where $x_1 \geq x_0$). We're looking for a function of time $x(t)$ that satisfies $x(0) = x_0$ and $x(\Delta t) = x_1$ (where $\Delta t \geq 0$). This allows us to determine the position at any time as well as velocity, acceleration, and higher-order time derivatives of $x(t)$. There are many such functions, so we'd like the minimum-time one that obeys the velocity constraint $|x'(t)| \leq v_{max}$ for all $0 \leq t \leq \Delta t$ (where $v_{max} > 0$). As you might suspect, the optimal trajectory maximizes the constraint by setting $x'(t) = v_{max}$. The corresponding position function is \begin{align*} \int_0^t x'(\tau) \, d\tau &= \int_0^t v_{max} \, d\tau\\ x(t) - x(0) &= v_{max} t\\ x(t) &= x_0 + v_{max} t. \end{align*} The final position constraint can be satisfied by choosing the duration \begin{align*} x_1 &= x_0 + v_{max} \Delta t\\ \Delta t &= \frac{x_1 - x_0}{v_{max}}. \end{align*}

The following snippet implements this velocity-limited trajectory generator.

In [ ]:
def vel_traj_gen(x0, x1, vmax):
    dt = (x1 - x0) / vmax
    return x0, vmax, dt


traj_x0, traj_v, traj_dt = vel_traj_gen(-20, 80, 30)
t = np.linspace(0, traj_dt, 100)

fig, ax = plt.subplots(1, 2, figsize=(12, 4))

ax[0].set_title('Velocity-Limited Trajectory Velocity')
ax[0].set_xlabel('time [s]')
ax[0].set_ylabel('velocity [in/s]')
ax[0].plot(t, np.full(t.shape, traj_v)) 

ax[1].set_title('Velocity-Limited Trajectory Position')
ax[1].set_xlabel('time [s]')
ax[1].set_ylabel('position [in]')
ax[1].plot(t, traj_x0 + traj_v * t)

The assumption so far is that the robot can execute the trajectory generated. However, it takes time for the robot to get from rest to high velocity. A better model of the robot's motion includes a maximum acceleration limit $|x''(t)| \leq a_{max}$ for all $0 \leq t \leq \Delta t$ (where $a_{max} > 0$). In this case, the optimal trajectory from $x_0$ to $x_1$ has multiple phases: acceleration, coast, and deceleration. The first and last phases maximize acceleration magnitude, while the second phase maximizes velocity. To save room, we will use the standard constant-velocity and constant-acceleration formulas. These can all be derived with rudimentary calculus as shown in the previous paragraph.

The first phase has acceleration $a_{max}$, and the duration is \begin{align*} v_{max} &= 0 + a_{max} \Delta t_1\\ \Delta t_1 &= \frac{v_{max}}{a_{max}}. \end{align*} The third phase has acceleration $-a_{max}$, and the duration is \begin{align*} 0 &= v_{max} - a_{max} \Delta t_3\\ \Delta t_3 &= \frac{v_{max}}{a_{max}}. \end{align*} which is the same as the first. This accords with the symmetry of the two phases. To compute the second phase duration, we massage the second position constraint into \begin{align*} x_1 &= x_0 + \frac{1}{2} a_{max} \Delta t_1^2 + v_{max} \Delta t_2 + v_{max} \Delta t_3 - \frac{1}{2} a_{max} \Delta t_3^2\\ &= x_0 + v_{max} \Delta t_2 + \frac{v_{max}^2}{a_{max}}\\ \Delta t_2 &= \frac{x_1 - x_0}{v_{max}} - \frac{v_{max}}{a_{max}}. \end{align*} But there's a potential problem here. What if the computed $\Delta t_2$ is negative? In this case, the robot doesn't have time to reach the maximum velocity and goes straight from acceleration into deceleration. As before, the duration of the two phases are $\Delta t_1 = \Delta t_3 = \Delta t$. The time is \begin{align*} x_1 &= x_0 + \frac{1}{2} a_{max} \Delta t_1^2 + a_{max} \Delta t_3^2 - \frac{1}{2} a_{max} \Delta t_3^2\\ &= x_0 + a_{max} \Delta t_1^2\\ \Delta t_1 &= \sqrt{\frac{x_1 - x_0}{a_{max}}} \end{align*}

With both the normal and degenerate case covered, we can implement the acceleration-limited trajectory generation.

In [ ]:
def accel_traj_gen(x0, x1, vmax, amax):
    dx = x1 - x0
    if vmax / amax < dx / vmax:
        # normal trajectory
        dt1 = vmax / amax
        dt2 = dx / vmax - vmax / amax
        dt3 = dt1
        return x0, (
            (amax, dt1),
            (0, dt2),
            (-amax, dt3)
        )
    else:
        # degenerate trajectory
        dt1 = np.sqrt(dx / amax)
        dt2 = dt1
        return x0, (
            (amax, dt1), 
            (-amax, dt2)
        )

For the trajectory to be useful, we need a few utilities. Ignore @vectorize_tail; it's just for plotting.

In [ ]:
@vectorize_tail
def accel_traj_get_accel(traj, t):
    _, phases = traj
    for a, dt in phases:
        if t < dt:
            return a
        
        t -= dt
    return a

        
@vectorize_tail
def accel_traj_get_vel(traj, t):
    _, phases = traj
    v0 = 0
    for a, dt in phases:
        if t < dt:
            return v0 + a * t
        
        v0 += a * dt
        
        t -= dt
    return v0
    
    
@vectorize_tail
def accel_traj_get_pos(traj, t):
    x0, phases = traj
    v0 = 0
    for a, dt in phases:
        if t < dt:
            return x0 + v0 * t + a * t**2 / 2
        
        x0 += v0 * dt + a * dt**2 / 2
        v0 += a * dt
        
        t -= dt 
    return x0


def accel_traj_duration(traj):
    _, phases = traj
    duration = 0
    for _, dt in phases:
        duration += dt
    return duration

Finally, we can make some plots.

In [ ]:
traj = accel_traj_gen(-20, 80, 30, 30)

t = np.linspace(0, accel_traj_duration(traj), 100)

fig, ax = plt.subplots(1, 3, figsize=(18, 4))

ax[0].set_title('Acceleration-Limited Trajectory Acceleration')
ax[0].set_xlabel('time [s]')
ax[0].set_ylabel('acceleration [in/s^2]')
ax[0].plot(t, accel_traj_get_accel(traj, t))

ax[1].set_title('Acceleration-Limited Trajectory Velocity')
ax[1].set_xlabel('time [s]')
ax[1].set_ylabel('velocity [in/s]')
ax[1].plot(t, accel_traj_get_vel(traj, t))

ax[2].set_title('Acceleration-Limited Trajectory Position')
ax[2].set_xlabel('time [s]')
ax[2].set_ylabel('position [in]')
ax[2].plot(t, accel_traj_get_pos(traj, t))

Try adjusting the parameters to get a degenerate trajectory with only two segments.

Splines

Of course, we don't want our robot to just travel in straight lines. Our next objective is to find a path between two locations on the field. Let's assume these locations correspond with vectors $(x_0, y_0)$ and $(x_1, y_1)$ in a fixed coordinate frame. The simplest path between these points is a line, of course. However, a line isn't always suitable. Perhaps there is a field element in the way, or you want to control the direction with which the robot approaches the goal. These requirements motivate a more flexible kind of path that can bend.

One common shape is a polynomial spline. The general 2D cubic spline has the form \begin{align*} x(u) &= a_x u^3 + b_x u^2 + c_x u + d_x\\ y(u) &= a_y u^3 + b_y u^2 + c_y u + d_y \end{align*} where $0 \leq u \leq 1$. To make sure the curve hits our vectors, we impose the constraints \begin{align*} x(0) &= x_0 & x(1) &= x_1\\ y(0) &= y_0 & y(1) &= y_1. \end{align*} And now the extra flexibility of the spline enables us to specify the begin and end tangents, $(x'_0, y'_0)$ and $(x'_1, y'_1)$. These requirements give the derivative constraints \begin{align*} x'(0) &= x'_0 & x'(1) &= x'_1\\ y'(0) &= y'_0 & y'(1) &= y'_1. \end{align*} At this point, you may have noticed that $x$ and $y$ seem independent. Indeed, the two splines can be computed separately. Let's consider the $x$ one and suppress the subscripts on the coefficients. The derivative is $$ x'(u) = 3 a u^2 + 2 b u + c. $$ Evaluating all of the constraints, we obtain the system of equations \begin{align*} x_0 &= d\\ x'_0 &= c\\ x_1 &= a + b + c + d\\ x'_1 &= 3a + 2b + c. \end{align*} As an exercise, you can verify that the solution is \begin{align*} a &= 2x_0 + x'_0 - 2x_1 + x'_1\\ b &= -3x_0 - 2x'_0 + 3x_1 - x'_1\\ c &= x'_0\\ d &= x_0. \end{align*}

All right, we can write methods to fit spline parameters and give spline values and derivatives.

In [ ]:
def spline_fit(x0, dx0, x1, dx1):
    a = 2 * x0 + dx0 - 2 * x1 + dx1
    b = -3 * x0 - 2 * dx0 + 3 * x1 - dx1
    c = dx0
    d = x0
    return a, b, c, d

def spline_get(spline, u):
    a, b, c, d = spline
    return a * u**3 + b * u**2 + c * u + d

def spline_deriv(spline, u):
    a, b, c, d = spline
    return 3 * a * u**2 + 2 * b * u + c

Let's give this a quick test.

In [ ]:
x_spline = spline_fit(0, 36, 24, 30)
y_spline = spline_fit(0, -24, 24, -9)
u = np.linspace(0, 1, 100)

plt.title('Cubic Spline')
plt.xlabel('x [in]')
plt.ylabel('y [in]')

plt.plot(spline_get(x_spline, u), spline_get(y_spline, u))

print(spline_get(x_spline, 0), spline_deriv(x_spline, 0), spline_get(x_spline, 1), spline_deriv(x_spline, 1))
print(spline_get(y_spline, 0), spline_deriv(y_spline, 0), spline_get(y_spline, 1), spline_deriv(y_spline, 1))

We have a spline now, but we need one more step before we can make a spline trajectory. We'd like a spline function with displacement $s$ along the path as its argument, but we instead have a meaningless spline parameter $u$. Mathematically, the distance can be computed with the integral $$ s(u) = \int_0^u \sqrt{\left( \frac{dx}{d\upsilon} \right)^2 + \left( \frac{dy}{d\upsilon} \right)^2} \, d\upsilon $$ where the dummy variable $\upsilon$ replaces $u$ in the integrand. All we need to do is invert this to get $u(s)$. This is tricky to do analytically, but fortunately, the inverse exists and we can approximate it in code.

In [ ]:
upsilon = np.linspace(0, 1, 100)
dupsilon = upsilon[1] - upsilon[0]
integrand = np.sqrt(
    spline_deriv(x_spline, upsilon)**2 + 
    spline_deriv(y_spline, upsilon)**2
)

sums = np.zeros_like(upsilon)
last_sum = 0
for i in range(len(upsilon)):
    sums[i] = last_sum + integrand[i] * dupsilon
    last_sum = sums[i]


@np.vectorize
def disp_of_spline_param(u):
    for i in range(len(upsilon)):
        if u < upsilon[i]:
            return sums[i]
    

u = np.linspace(0, 1, 100)

fig, ax = plt.subplots(1, 2, figsize=(12, 4))

ax[0].set_title('Displacement vs. Spline Parameter')
ax[0].set_xlabel('u')
ax[0].set_ylabel('s [in]')
ax[0].plot(u, disp_of_spline_param(u))

ax[1].set_title('Spline Parameter vs. Displacement')
ax[1].set_xlabel('s [in]')
ax[1].set_ylabel('u')
ax[1].plot(disp_of_spline_param(u), u)

The left plot is a rough approximation of $s(u)$ computed by estimating the integral, and the right plot is the approximation's inverse. The method below implements this function by finding the closest integral sum.

In [ ]:
@np.vectorize
def spline_param_of_disp(s):
    for i in range(len(sums)):
        if s < sums[i]:
            return upsilon[i]

Note the similarities between the two methods. Now we put everything together and generate a trajectory for the spline!

In [ ]:
length = sums[-1]
traj = accel_traj_gen(0, length, 30, 30)
t = np.linspace(0, accel_traj_duration(traj), 100)

s = accel_traj_get_pos(traj, t)

u = spline_param_of_disp(s)

x = spline_get(x_spline, u)
y = spline_get(y_spline, u)

plt.title('Spline Trajectory Position')
plt.xlabel('time [s]')
plt.ylabel('position [in]')
plt.plot(t, x, label='x')
plt.plot(t, y, label='y')
plt.legend()

Each component of the trajectory is the composition of three functions: $(x(u(s(t))), y(u(s(t))))$. We can find the velocity by differentiating with respect to $t$. For the $x$-component, this gives \begin{align*} x'(t) &= \frac{d}{dt} x(u(s(t))))\\ &= x'(u(s(t)) \, u'(s(t)) \, s'(t). \end{align*} The only derivative missing is $u'(s)$. Recall that we couldn't find an analytical expression for $u(s)$. Nevertheless, we can find the derivative from the inverse $s(u)$. We have \begin{align*} u'(s) &= [s'(u)]^{-1}\\ &= \left[\frac{d}{du} \int_0^u \sqrt{\left( \frac{dx}{d\upsilon} \right)^2 + \left( \frac{dy}{d\upsilon} \right)^2} \, d\upsilon \right]^{-1}\\ &= \left[\sqrt{\left( \frac{dx}{du} \right)^2 + \left( \frac{dy}{du} \right)^2}\right]^{-1}. \end{align*}

In [ ]:
def spline_param_of_disp_deriv(x_spline, y_spline, u):
    return 1.0 / np.sqrt(spline_deriv(x_spline, u)**2 + spline_deriv(y_spline, u)**2)


dsdt = accel_traj_get_vel(traj, t)

duds = spline_param_of_disp_deriv(x_spline, y_spline, u)

dxdu = spline_deriv(x_spline, u)
dydu = spline_deriv(y_spline, u)

dxdt = dxdu * duds * dsdt
dydt = dydu * duds * dsdt


plt.title('Spline Trajectory Velocity')
plt.xlabel('time [s]')
plt.ylabel('velocity [in/s]')
plt.plot(t, dxdt, label='x')
plt.plot(t, dydt, label='y')
plt.legend()

As an advanced exercise, try to compute the spline trajectory acceleration.