Advanced Trajectories: Spline, B-Spline & Time-Optimal Planning

In the previous posts, we built up from basic motion profiles to velocity blending. But when the robot needs to follow complex multi-waypoint paths — spray painting curved surfaces, CNC machining 3D surfaces, or time-optimal movements — we need advanced techniques: Spline interpolation and Time-Optimal Planning.

This post dives deep into cubic spline, B-spline, NURBS, and especially the TOPPRA algorithm — the state-of-the-art for time-optimal trajectory parameterization.

Cubic Spline Interpolation

The Problem

Given $n$ waypoints, we want to create a smooth curve (C2 continuous) that passes through every point. A single high-degree polynomial (degree $n-1$) suffers from Runge's phenomenon — large oscillations between points.

Solution: Piecewise Cubic Spline

Instead of one high-degree polynomial, use many cubic polynomials, one between each pair of adjacent waypoints:

S_i(t) = a_i + b_i(t - t_i) + c_i(t - t_i)² + d_i(t - t_i)³

With constraints:

• Interpolation: $S_i(t_i) = q_i$, $S_i(t_{i+1}) = q_{i+1}$
• C1: $S_i'(t_{i+1}) = S_{i+1}'(t_{i+1})$ (velocity continuity)
• C2: $S_i''(t_{i+1}) = S_{i+1}''(t_{i+1})$ (acceleration continuity)

import numpy as np
from scipy.interpolate import CubicSpline
import matplotlib.pyplot as plt

# 10 waypoints for robot arm (joint 1 position)
t_waypoints = np.array([0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5])
q_waypoints = np.array([0, 15, 45, 60, 50, 30, 55, 70, 45, 10])  # degrees

# Cubic spline interpolation
cs = CubicSpline(t_waypoints, q_waypoints, bc_type='clamped')

# Dense evaluation
t_dense = np.linspace(0, 4.5, 1000)
q_dense = cs(t_dense)
qd_dense = cs(t_dense, 1)
qdd_dense = cs(t_dense, 2)
qddd_dense = cs(t_dense, 3)

fig, axes = plt.subplots(4, 1, figsize=(14, 10), sharex=True)

axes[0].plot(t_dense, q_dense, 'b-', linewidth=2)
axes[0].scatter(t_waypoints, q_waypoints, c='red', s=80, zorder=5,
               label='Waypoints')
axes[0].set_ylabel('Position (deg)')
axes[0].set_title('Cubic Spline Through 10 Waypoints')
axes[0].legend()
axes[0].grid(True)

axes[1].plot(t_dense, qd_dense, 'r-', linewidth=2)
axes[1].set_ylabel('Velocity (deg/s)')
axes[1].grid(True)

axes[2].plot(t_dense, qdd_dense, 'g-', linewidth=2)
axes[2].set_ylabel('Acceleration (deg/s²)')
axes[2].grid(True)

axes[3].plot(t_dense, qddd_dense, 'm-', linewidth=1.5)
axes[3].set_ylabel('Jerk (deg/s³)')
axes[3].set_xlabel('Time (s)')
axes[3].grid(True)

plt.tight_layout()
plt.savefig('cubic_spline.png', dpi=150)
plt.show()

Multi-Joint Spline Trajectory

import roboticstoolbox as rtb
import numpy as np
from scipy.interpolate import CubicSpline
import matplotlib.pyplot as plt

ur5e = rtb.models.UR5()

waypoints = np.array([
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],
    [np.pi/6, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, np.pi/6],
    [np.pi/3, -np.pi/6, np.pi/6, -np.pi/4, np.pi/4, np.pi/3],
    [np.pi/4, -np.pi/3, np.pi/2, -np.pi/2, np.pi/3, np.pi/6],
    [np.pi/6, -np.pi/4, np.pi/3, -np.pi/3, np.pi/4, np.pi/4],
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],
])

t_way = np.linspace(0, 5, len(waypoints))
t_dense = np.linspace(0, 5, 500)
q_traj = np.zeros((len(t_dense), 6))

for j in range(6):
    cs = CubicSpline(t_way, waypoints[:, j], bc_type='clamped')
    q_traj[:, j] = cs(t_dense)

tcp_positions = []
for q in q_traj:
    T = ur5e.fkine(q)
    tcp_positions.append(T.t)
tcp_positions = np.array(tcp_positions)

fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
ax.plot(tcp_positions[:, 0], tcp_positions[:, 1], tcp_positions[:, 2],
        'b-', linewidth=2, label='Spline TCP path')

for q in waypoints:
    T = ur5e.fkine(q)
    ax.scatter(*T.t, c='red', s=80, zorder=5)

ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Multi-Joint Cubic Spline — TCP Path')
ax.legend()
plt.tight_layout()
plt.savefig('multi_joint_spline.png', dpi=150)
plt.show()

B-Spline — Smoother Curves, Non-Interpolating

Differences from Cubic Spline

B-Spline does not pass through control points but is "attracted" toward them — creating curves that are smoother and easier to control.

from scipy.interpolate import make_interp_spline
import numpy as np
import matplotlib.pyplot as plt

control_points = np.array([
    [0.0, 0.0], [0.1, 0.3], [0.3, 0.5], [0.5, 0.2],
    [0.7, 0.6], [0.9, 0.4], [1.0, 0.0],
])

t_ctrl = np.linspace(0, 1, len(control_points))
bspline = make_interp_spline(t_ctrl, control_points, k=3)

t_dense = np.linspace(0, 1, 500)
path = bspline(t_dense)

cs_x = CubicSpline(t_ctrl, control_points[:, 0])
cs_y = CubicSpline(t_ctrl, control_points[:, 1])

fig, ax = plt.subplots(figsize=(10, 6))
ax.plot(path[:, 0], path[:, 1], 'b-', linewidth=2, label='B-Spline')
ax.plot(cs_x(t_dense), cs_y(t_dense), 'r--', linewidth=2, label='Cubic Spline')
ax.scatter(control_points[:, 0], control_points[:, 1],
           c='green', s=100, zorder=5, label='Control Points')
ax.set_title('B-Spline vs Cubic Spline')
ax.legend()
ax.grid(True)
ax.set_aspect('equal')
plt.tight_layout()
plt.savefig('bspline_vs_cubic.png', dpi=150)
plt.show()

NURBS — Non-Uniform Rational B-Splines

NURBS extends B-Splines with:

• Non-uniform: Uneven knot vector — allows concentrating resolution at complex curves
• Rational: Each control point has a weight — enables exact representation of conic sections (circles, ellipses)

NURBS is the standard in CAD/CAM and CNC robotics.

Time-Optimal Trajectory Planning — TOPPRA

The Problem

Given a geometric path (from a spline), find the time parameterization $s(t)$ such that:

• The robot follows the path as fast as possible
• Without violating velocity, acceleration, or torque constraints

TOPPRA Algorithm (Pham & Pham, 2018)

TOPPRA (Time-Optimal Path Parameterization based on Reachability Analysis) is state-of-the-art:

• Solves in $O(n)$ (linear in number of points)
• Handles diverse constraints: velocity, acceleration, torque, friction
• Always finds the globally optimal solution

import numpy as np
import toppra
import toppra.constraint as constraint
import toppra.algorithm as algo
import matplotlib.pyplot as plt

# Define geometric path (spline through waypoints)
waypoints = np.array([
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],
    [np.pi/6, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, np.pi/6],
    [np.pi/3, -np.pi/6, np.pi/6, -np.pi/4, np.pi/4, np.pi/3],
    [np.pi/4, -np.pi/3, np.pi/2, -np.pi/2, np.pi/3, np.pi/6],
    [np.pi/6, -np.pi/4, np.pi/3, -np.pi/3, np.pi/4, np.pi/4],
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],
])

ss = np.linspace(0, 1, len(waypoints))
path = toppra.SplineInterpolator(ss, waypoints)

# Velocity and acceleration constraints
vlim = np.array([[-2.0, 2.0]] * 2 + [[-3.0, 3.0]] * 4)
alim = np.array([[-5.0, 5.0]] * 2 + [[-10.0, 10.0]] * 4)

pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(alim)

# Run TOPPRA
instance = algo.TOPPRA([pc_vel, pc_acc], path,
                        parametrizer="ParametrizeConstAccel")
trajectory = instance.compute_trajectory()

if trajectory is not None:
    print(f"Time-optimal duration: {trajectory.duration:.3f} s")

    t_grid = np.linspace(0, trajectory.duration, 500)
    q_traj = trajectory(t_grid)
    qd_traj = trajectory(t_grid, 1)
    qdd_traj = trajectory(t_grid, 2)

    fig, axes = plt.subplots(3, 1, figsize=(14, 8), sharex=True)

    for j in range(6):
        axes[0].plot(t_grid, np.degrees(q_traj[:, j]), label=f'J{j+1}')
        axes[1].plot(t_grid, qd_traj[:, j], label=f'J{j+1}')
        axes[2].plot(t_grid, qdd_traj[:, j], label=f'J{j+1}')

    axes[0].set_ylabel('Position (deg)')
    axes[0].set_title(f'TOPPRA Time-Optimal Trajectory (T={trajectory.duration:.2f}s)')
    axes[0].legend(ncol=3)
    axes[0].grid(True)

    axes[1].set_ylabel('Velocity (rad/s)')
    axes[1].grid(True)

    axes[2].set_ylabel('Acceleration (rad/s²)')
    axes[2].set_xlabel('Time (s)')
    axes[2].grid(True)

    plt.tight_layout()
    plt.savefig('toppra_trajectory.png', dpi=150)
    plt.show()

Minimum-Time vs Minimum-Jerk Tradeoff

Application: Spray Painting Complex Shapes

import numpy as np
import matplotlib.pyplot as plt

def generate_painting_path(surface_func, u_range, v_range,
                           n_passes, standoff=0.05):
    """Generate spray painting path over a surface."""
    v_values = np.linspace(v_range[0], v_range[1], n_passes)
    u_dense = np.linspace(u_range[0], u_range[1], 50)
    all_points = []

    for i, v in enumerate(v_values):
        u_range_dir = u_dense if i % 2 == 0 else u_dense[::-1]
        for u in u_range_dir:
            point = surface_func(u, v)
            point[2] += standoff
            all_points.append(point)

    return np.array(all_points)

def curved_surface(u, v):
    return np.array([u, v, 0.1 * np.sin(2*np.pi*u) * np.cos(np.pi*v)])

path = generate_painting_path(curved_surface, (0, 0.5), (0, 0.3), 8, 0.03)

fig = plt.figure(figsize=(12, 8))
ax = fig.add_subplot(111, projection='3d')

u_grid = np.linspace(0, 0.5, 50)
v_grid = np.linspace(0, 0.3, 50)
U, V = np.meshgrid(u_grid, v_grid)
Z = 0.1 * np.sin(2*np.pi*U) * np.cos(np.pi*V)
ax.plot_surface(U, V, Z, alpha=0.2, color='gray')

ax.plot(path[:, 0], path[:, 1], path[:, 2], 'r-', linewidth=1.5,
        label='Painting path')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Spray Painting — Spline Path Over Curved Surface')
ax.legend()
plt.tight_layout()
plt.savefig('painting_path.png', dpi=150)
plt.show()

References

• Pham, H., Pham, Q.C. "A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis." IEEE Transactions on Robotics, 2018. DOI: 10.1109/TRO.2018.2819195 | arXiv:1707.07239
• Biagiotti, L., Melchiorri, C. Trajectory Planning for Automatic Machines and Robots. Springer, 2008. DOI: 10.1007/978-3-540-85629-0
• Piegl, L., Tiller, W. The NURBS Book. Springer, 1997. DOI: 10.1007/978-3-642-59223-2

Conclusion

Advanced trajectory planning techniques:

• Cubic Spline — smooth C2 interpolation through waypoints
• B-Spline — approximating, local control, smoother than interpolating spline
• NURBS — exact representation of complex curves, standard in CNC
• TOPPRA — time-optimal parameterization, O(n), state-of-the-art

In the final post of this series, we will integrate all knowledge into MoveIt2, URScript and real robot deployment.

Related Posts

• Motion Profiles: Trapezoidal, S-Curve, Polynomial — Foundation for time parameterization
• MoveIt2 Motion Planning — Framework integrating trajectory planning
• Robot Simulation with MuJoCo — Test trajectories in simulation before deployment