Motion Profiles: Trapezoidal, S-Curve & Polynomial Trajectories

In the previous post on blending, we saw how velocity profiles affect motion quality. Now it is time to dive deep into the 3 most important motion profiles in robot control: Trapezoidal, S-Curve, and Polynomial. Understanding them is the key to creating motion that is simultaneously fast, smooth, and safe for the mechanism.

Overview of 3 Motion Profile Types

Trapezoidal Velocity Profile

The 3-Phase Principle

The trapezoidal profile consists of 3 phases:

1. Acceleration phase: $\dot{q}$ increases linearly, $\ddot{q}$ = constant
2. Cruise phase: $\dot{q}$ = constant, $\ddot{q}$ = 0
3. Deceleration phase: $\dot{q}$ decreases linearly, $\ddot{q}$ = constant (negative)

The Problem: Infinite Jerk

At phase transitions, acceleration jumps abruptly — jerk ($\dddot{q}$) is infinite. This causes:

• Structural vibration
• Faster gear wear
• Noise at joints
• Position error due to structural elasticity

Implementation from Scratch

import numpy as np
import matplotlib.pyplot as plt

def trapezoidal_profile(q0, qf, v_max, a_max, dt=0.001):
    """
    Generate trapezoidal velocity profile.

    Args:
        q0: start position
        qf: end position
        v_max: maximum velocity
        a_max: maximum acceleration
        dt: time step
    """
    distance = abs(qf - q0)
    sign = np.sign(qf - q0)

    t_acc = v_max / a_max
    d_acc = 0.5 * a_max * t_acc**2

    if 2 * d_acc > distance:
        # Triangular profile (v_max not reached)
        t_acc = np.sqrt(distance / a_max)
        v_peak = a_max * t_acc
        t_cruise = 0
        T = 2 * t_acc
    else:
        # Full trapezoidal
        d_cruise = distance - 2 * d_acc
        t_cruise = d_cruise / v_max
        T = 2 * t_acc + t_cruise
        v_peak = v_max

    t = np.arange(0, T + dt, dt)
    q = np.zeros_like(t)
    qd = np.zeros_like(t)
    qdd = np.zeros_like(t)
    qddd = np.zeros_like(t)

    for i, ti in enumerate(t):
        if ti <= t_acc:
            qdd[i] = a_max
            qd[i] = a_max * ti
            q[i] = q0 + sign * 0.5 * a_max * ti**2
        elif ti <= t_acc + t_cruise:
            qdd[i] = 0
            qd[i] = v_peak
            q[i] = q0 + sign * (d_acc + v_peak * (ti - t_acc))
        else:
            t_dec = ti - t_acc - t_cruise
            qdd[i] = -a_max
            qd[i] = v_peak - a_max * t_dec
            q[i] = q0 + sign * (d_acc + v_peak * t_cruise +
                    v_peak * t_dec - 0.5 * a_max * t_dec**2)

    qd *= sign
    qdd *= sign
    qddd[1:] = np.diff(qdd) / dt

    return t, q, qd, qdd, qddd

t_trap, q_trap, qd_trap, qdd_trap, qddd_trap = trapezoidal_profile(
    q0=0, qf=np.pi/2, v_max=2.0, a_max=5.0
)

fig, axes = plt.subplots(4, 1, figsize=(12, 10), sharex=True)
axes[0].plot(t_trap, np.degrees(q_trap), 'b-', linewidth=2)
axes[0].set_ylabel('Position (deg)')
axes[0].set_title('Trapezoidal Velocity Profile')
axes[0].grid(True)

axes[1].plot(t_trap, qd_trap, 'r-', linewidth=2)
axes[1].set_ylabel('Velocity (rad/s)')
axes[1].grid(True)

axes[2].plot(t_trap, qdd_trap, 'g-', linewidth=2)
axes[2].set_ylabel('Acceleration (rad/s²)')
axes[2].grid(True)

axes[3].plot(t_trap, qddd_trap, 'm-', linewidth=1)
axes[3].set_ylabel('Jerk (rad/s³)')
axes[3].set_xlabel('Time (s)')
axes[3].set_title('Note: Infinite jerk spikes at phase transitions!')
axes[3].grid(True)

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

S-Curve Profile (7-Segment)

The 7-Phase Principle

The S-Curve solves the infinite jerk problem by adding jerk ramp phases:

1. Increasing jerk — acceleration ramps up
2. Constant acceleration — jerk = 0
3. Decreasing jerk — acceleration ramps down to 0
4. Cruise — constant velocity
5. Increasing jerk (negative) — start deceleration
6. Constant acceleration (negative) — steady deceleration
7. Decreasing jerk (positive) — acceleration returns to 0

def s_curve_profile(q0, qf, v_max, a_max, j_max, dt=0.001):
    """Generate S-curve (7-segment) velocity profile."""
    distance = abs(qf - q0)
    sign = np.sign(qf - q0)

    t_j = a_max / j_max
    v_at_jerk = 0.5 * j_max * t_j**2
    t_a = (v_max - 2 * v_at_jerk) / a_max

    if t_a < 0:
        t_a = 0
        t_j = np.sqrt(v_max / j_max)

    # Numerical simulation
    t_list, q_list, qd_list, qdd_list, qddd_list = [0], [q0], [0], [0], [0]
    accel, vel, pos = 0, 0, q0

    # Phase 1: Increasing jerk
    t_phase = 0
    while t_phase < t_j:
        jerk = j_max
        accel += jerk * dt
        accel = min(accel, a_max)
        vel += accel * dt
        vel = min(vel, v_max)
        pos += sign * vel * dt
        t_phase += dt
        t_list.append(t_list[-1] + dt)
        q_list.append(pos)
        qd_list.append(sign * vel)
        qdd_list.append(sign * accel)
        qddd_list.append(sign * jerk)

    # Phase 2: Constant acceleration
    t_phase = 0
    while t_phase < t_a and vel < v_max:
        vel += accel * dt
        vel = min(vel, v_max)
        pos += sign * vel * dt
        t_phase += dt
        t_list.append(t_list[-1] + dt)
        q_list.append(pos)
        qd_list.append(sign * vel)
        qdd_list.append(sign * accel)
        qddd_list.append(0)

    # Phase 3: Decreasing jerk
    t_phase = 0
    while t_phase < t_j and accel > 0:
        accel -= j_max * dt
        accel = max(accel, 0)
        vel += accel * dt
        vel = min(vel, v_max)
        pos += sign * vel * dt
        t_phase += dt
        t_list.append(t_list[-1] + dt)
        q_list.append(pos)
        qd_list.append(sign * vel)
        qdd_list.append(sign * accel)
        qddd_list.append(-sign * j_max)

    # Phase 4: Cruise
    remaining = distance - 2 * abs(pos - q0)
    if remaining > 0 and vel > 0:
        t_cruise = remaining / vel
        t_phase = 0
        while t_phase < t_cruise:
            pos += sign * vel * dt
            t_phase += dt
            t_list.append(t_list[-1] + dt)
            q_list.append(pos)
            qd_list.append(sign * vel)
            qdd_list.append(0)
            qddd_list.append(0)

    # Phases 5-7: Deceleration (mirror)
    for phase_jerk, phase_time in [(j_max, t_j), (0, t_a), (-j_max, t_j)]:
        t_phase = 0
        target_time = phase_time
        while t_phase < target_time and vel > dt:
            accel -= j_max * dt if phase_jerk == j_max else (
                0 if phase_jerk == 0 else -j_max * dt)
            vel += accel * dt
            vel = max(vel, 0)
            pos += sign * vel * dt
            t_phase += dt
            t_list.append(t_list[-1] + dt)
            q_list.append(pos)
            qd_list.append(sign * vel)
            qdd_list.append(sign * accel)
            qddd_list.append(sign * (-phase_jerk))

    return (np.array(t_list), np.array(q_list),
            np.array(qd_list), np.array(qdd_list), np.array(qddd_list))

Polynomial Trajectories

Cubic Polynomial (4 constraints)

def cubic_poly(q0, qf, qd0, qdf, T, dt=0.001):
    """Cubic polynomial: matches position and velocity at endpoints."""
    a0 = q0
    a1 = qd0
    a2 = (3*(qf - q0) - (2*qd0 + qdf)*T) / T**2
    a3 = (-2*(qf - q0) + (qd0 + qdf)*T) / T**3

    t = np.arange(0, T + dt, dt)
    q = a0 + a1*t + a2*t**2 + a3*t**3
    qd = a1 + 2*a2*t + 3*a3*t**2
    qdd = 2*a2 + 6*a3*t
    qddd = np.full_like(t, 6*a3)

    return t, q, qd, qdd, qddd

Quintic Polynomial (6 constraints)

def quintic_poly(q0, qf, T, dt=0.001):
    """Quintic polynomial: zero velocity and acceleration at endpoints."""
    a0 = q0
    a3 = 10*(qf - q0) / T**3
    a4 = -15*(qf - q0) / T**4
    a5 = 6*(qf - q0) / T**5

    t = np.arange(0, T + dt, dt)
    q = a0 + a3*t**3 + a4*t**4 + a5*t**5
    qd = 3*a3*t**2 + 4*a4*t**3 + 5*a5*t**4
    qdd = 6*a3*t + 12*a4*t**2 + 20*a5*t**3
    qddd = 6*a3 + 24*a4*t + 60*a5*t**2

    return t, q, qd, qdd, qddd

When to Use Which?

S-Curve Eliminates Vibration — Demo

def simulate_vibration(qdd_profile, t, mass=1.0, stiffness=500.0, damping=2.0):
    """Simulate structural vibration from acceleration profile."""
    omega_n = np.sqrt(stiffness / mass)
    zeta = damping / (2 * np.sqrt(mass * stiffness))

    dt = t[1] - t[0]
    x = np.zeros_like(t)
    xd = np.zeros_like(t)

    for i in range(1, len(t)):
        F = mass * qdd_profile[i]
        xdd = F/mass - 2*zeta*omega_n*xd[i-1] - omega_n**2*x[i-1]
        xd[i] = xd[i-1] + xdd * dt
        x[i] = x[i-1] + xd[i] * dt

    return x

fig, axes = plt.subplots(2, 1, figsize=(12, 8))

vib_trap = simulate_vibration(qdd_trap, t_trap)
axes[0].plot(t_trap, vib_trap * 1000, 'b-', linewidth=1.5)
axes[0].set_title('Vibration from Trapezoidal Profile')
axes[0].set_ylabel('Displacement (mm)')
axes[0].grid(True)

t_s, q_s, qd_s, qdd_s, qddd_s = s_curve_profile(0, np.pi/2, 2.0, 5.0, 30.0)
vib_s = simulate_vibration(qdd_s, t_s)
axes[1].plot(t_s, vib_s * 1000, 'r-', linewidth=1.5)
axes[1].set_title('Vibration from S-Curve Profile')
axes[1].set_ylabel('Displacement (mm)')
axes[1].set_xlabel('Time (s)')
axes[1].grid(True)

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

print(f"Trapezoidal max vibration: {np.max(np.abs(vib_trap))*1000:.3f} mm")
print(f"S-Curve max vibration: {np.max(np.abs(vib_s))*1000:.3f} mm")

References

• Biagiotti, L., Melchiorri, C. Trajectory Planning for Automatic Machines and Robots. Springer, 2008. DOI: 10.1007/978-3-540-85629-0
• Haschke, R., Weitnauer, E., Ritter, H. "On-Line Planning of Time-Optimal, Jerk-Limited Trajectories." IROS, 2008. DOI: 10.1109/IROS.2008.4650924
• Lynch, K.M., Park, F.C. Modern Robotics: Mechanics, Planning, and Control. Cambridge, 2017. Link

Conclusion

Three motion profile types serve different needs:

• Trapezoidal — simple, fast to compute, but infinite jerk causes vibration
• S-Curve — bounded jerk, significantly reduces vibration, standard in industry
• Polynomial — mathematically smoothest, easy to customize constraints

In the next post, we will explore Spline, B-Spline and Time-Optimal Planning — advanced techniques for complex trajectories and time optimization.

Related Posts

• Velocity Blending — Blending uses these motion profiles
• Robot Simulation with MuJoCo — Test motion profiles in simulation
• PID Control for Robots — PID tracking different motion profiles