Stage 09 · Lesson 08/15

Trajectory Planning & Motion Generation · Lesson 08

Quintic Polynomial Trajectories

Est. 50–70 min

Why

Why this lesson matters

Real robot arms are never perfectly stiff. Links bend a little, gears have a little play, belts stretch, and the payload swings. A sudden change in acceleration (a large jerk) excites these springs and leaves the tool shaking after the joint has stopped — bad for precise placing, cameras, welding and anything carrying liquid. The quintic polynomial is the classic, simple fix used in many textbook and real planners.

By the end of this lesson you will be able to: set up the six conditions of a quintic and solve them; use the rest-to-rest quintic s(u) = 10u³ − 15u⁴ + 6u⁵; compare its peaks (15d/(8T) speed, 10d/(√3·T²) acceleration) with the cubic’s; explain the trade-off — smoother ends but a higher peak speed; see in a simulation when a quintic reduces wobble and when it doesn’t; and demonstrate it with a springy ruler on a servo.

An outdoor swimming pool with a 1 metre and a 3 metre diving springboard on a steel tower, surrounded by green trees
Springboards at an outdoor pool. A springboard keeps vibrating after the diver leaves — just as a light robot link keeps wobbling after a jerky stop.Photo: Tetzemann · CC BY 3.0 · Source: Wikimedia Commons · resized, converted to WebP

Concept

Six conditions, six coefficients

Two more wishes: acceleration at the ends

*Modern Robotics* (Section 9.2.2.1) explains the problem with the cubic: it does not force the end accelerations to zero, so the robot is asked for a jump in acceleration at t = 0 and t = T — an infinite jerk, “which may cause vibration of the robot.” The fix is to add the conditions q̈(0) = 0 and q̈(T) = 0. Six conditions need six coefficients, so the polynomial becomes fifth order:

Quintic polynomial and its six conditions · Text
q(t) = a0 + a1·t + a2·t² + a3·t³ + a4·t⁴ + a5·t⁵

q(0) = q0    q̇(0) = v0    q̈(0) = acc0
q(T) = qf    q̇(T) = vf    q̈(T) = accf

For the common rest-to-rest case (all start and end velocities and accelerations zero), with u = t/T and d = qf − q0, the answer is beautifully simple:

Rest-to-rest quintic · Text
s(u) = 10u³ − 15u⁴ + 6u⁵
q(t) = q0 + s(t/T) · d

peak speed        = 15·d / (8·T)      ≈ 1.875·d/T   at t = T/2
peak acceleration = 10·d / (√3·T²)    ≈ 5.77·d/T²
Cubic (Lesson 7)Quintic
Conditions4: position and velocity at both ends6: position, velocity and acceleration at both ends
Acceleration at start/end±6·d/T² (a jump from rest)0 (smooth start and stop)
Jerk at start/endInfinite (acceleration step)Finite
Peak speed1.5·d/T1.875·d/T (25% higher)
Peak acceleration6·d/T²≈ 5.77·d/T² (slightly lower, but in the middle of the speed-up)
Good forLight, stiff arms; simple codeFlexible links, delicate or liquid payloads, cameras

Visual

Visualise it: quintic versus cubic

Linear, cubic and quintic side by side. Only the quintic has zero acceleration at both ends — at the cost of a taller velocity peak.

Interactive · Motion profile comparator

Quintic polynomial rest-to-rest trajectoryJerk and smooth motion comparisonMulti-joint synchronization shared progress

Overlay linear, cubic, and quintic rest-to-rest profiles. Linear has velocity steps (harsh); cubic zeros velocity; quintic also zeros acceleration — lowest jerk among the three.

Position q(t)

Velocity q̇(t)

Acceleration q̈(t)

Jerk q⃛(t)

Profilepeak |q̇|peak |q̈|peak |jerk|
Linear36.00.0∞ (edge)
Cubic54.086.469.1
Quintic67.583.1345.6

Educational 1-DOF overlays — not full multi-DOF dynamics. Linear jerk shown as edge impulse in table.

Code lab

Code lab: quintic vs cubic, on paper and on a springy arm

quintic.py solves the 6×6 system, compares it with the formula, puts cubic and quintic side by side, finds the shortest T for both under the same limits, and then simulates a springy arm: the tool tip is a mass on a spring (natural frequency 3 Hz, 2% damping — example values) dragged along by the joint. It measures how much the tip still wobbles after the joint has stopped. Run python3 quintic.py.

quintic.py · Python
"""quintic.py — Stage 9, Lesson 8
Quintic (fifth-order) polynomial: six conditions -> six coefficients.
    q(0)=q0, q'(0)=v0, q''(0)=acc0,  q(T)=qf, q'(T)=vf, q''(T)=accf
Rest-to-rest case:  s(u) = 10u^3 - 15u^4 + 6u^5,  u = t/T   (Modern Robotics 9.2.2.1)
Part 1: solve the 6x6 system and compare with the rest-to-rest formula.
Part 2: cubic vs quintic for the same move - peaks and the end accelerations.
Part 3: shortest T under speed and acceleration limits, cubic vs quintic.
Part 4: why it matters - a springy arm (example: 3 Hz, light damping) after each move.
"""
import math


def solve(A, b):
    n = len(b)
    M = [row[:] + [b[i]] for i, row in enumerate(A)]
    for c in range(n):
        p = max(range(c, n), key=lambda r: abs(M[r][c]))
        M[c], M[p] = M[p], M[c]
        for r in range(c + 1, n):
            f = M[r][c] / M[c][c]
            M[r] = [x - f * y for x, y in zip(M[r], M[c])]
    x = [0.0] * n
    for r in range(n - 1, -1, -1):
        x[r] = (M[r][n] - sum(M[r][k] * x[k] for k in range(r + 1, n))) / M[r][r]
    return x


def quintic_coeffs(q0, v0, a0, qf, vf, af, T):
    A = [[1, 0, 0, 0, 0, 0],
         [0, 1, 0, 0, 0, 0],
         [0, 0, 2, 0, 0, 0],
         [1, T, T**2, T**3, T**4, T**5],
         [0, 1, 2*T, 3*T**2, 4*T**3, 5*T**4],
         [0, 0, 2, 6*T, 12*T**2, 20*T**3]]
    return solve(A, [q0, v0, a0, qf, vf, af])


def poly(c, t):
    q = sum(ci * t**i for i, ci in enumerate(c))
    v = sum(i * ci * t**(i - 1) for i, ci in enumerate(c) if i >= 1)
    a = sum(i * (i - 1) * ci * t**(i - 2) for i, ci in enumerate(c) if i >= 2)
    j = sum(i * (i - 1) * (i - 2) * ci * t**(i - 3) for i, ci in enumerate(c) if i >= 3)
    return q, v, a, j


Q0, QF, T = 30.0, 120.0, 1.5
d = QF - Q0
cq = quintic_coeffs(Q0, 0, 0, QF, 0, 0, T)
print("Part 1 - quintic coefficients, 30 -> 120 deg in 1.5 s, rest to rest")
print("  6x6 solve:", [round(x, 3) + 0.0 for x in cq])
print("  formula  :", [Q0, 0, 0, round(10*d/T**3, 3), round(-15*d/T**4, 3), round(6*d/T**5, 3)])

cc = [Q0, 0, 3*d/T**2, -2*d/T**3, 0, 0]
print("\nPart 2 - cubic vs quintic, same move")
print("            peak|v|   peak|a|   a(0)    a(T)    jerk at t=0")
for name, c in (("cubic", cc), ("quintic", cq)):
    vs = [abs(poly(c, T*n/2000)[1]) for n in range(2001)]
    acs = [abs(poly(c, T*n/2000)[2]) for n in range(2001)]
    print(f"{name:8s}  {max(vs):7.1f}  {max(acs):8.1f}  {poly(c, 0)[2]:6.1f}  {poly(c, T)[2]:6.1f}   {poly(c, 0)[3]:8.1f}")
print(f"formulas: cubic v 1.5d/T = {1.5*d/T:.1f}, a 6d/T^2 = {6*d/T**2:.1f};"
      f" quintic v 15d/(8T) = {15*d/(8*T):.1f}, a 10d/(sqrt(3)T^2) = {10*d/(math.sqrt(3)*T**2):.1f}")

print("\nPart 3 - shortest T with v <= 100 deg/s and a <= 200 deg/s^2")
VL, AL = 100.0, 200.0
for dd in (20.0, 90.0, 150.0):
    tc = max(1.5*dd/VL, math.sqrt(6*dd/AL))
    tq = max(15*dd/(8*VL), math.sqrt(10*dd/(math.sqrt(3)*AL)))
    print(f"  {dd:5.1f} deg: cubic {tc:.3f} s, quintic {tq:.3f} s")

print("\nPart 4 - springy arm: tip lags the joint like a mass on a spring (f = 3 Hz, damping 2%)")
W, Z = 2 * math.pi * 3.0, 0.02


def residual(c, Tm):
    """Simulate tip angle y with y'' = W^2 (q - y) - 2 Z W (y' - q'); return wobble after the move."""
    y, yd, dt, t, wob = c[0], 0.0, 0.0005, 0.0, 0.0
    while t < Tm + 2.0:
        if t <= Tm:
            q, qd, _, _ = poly(c, t)
        else:
            q, qd = QF, 0.0
        ydd = W * W * (q - y) - 2 * Z * W * (yd - qd)
        yd += ydd * dt
        y += yd * dt
        t += dt
        if t > Tm:
            wob = max(wob, abs(y - QF))
    return wob


for Tm in (0.6, 1.0, 1.5):
    c3 = [Q0, 0, 3*d/Tm**2, -2*d/Tm**3, 0, 0]
    c5 = quintic_coeffs(Q0, 0, 0, QF, 0, 0, Tm)
    print(f"  T = {Tm:.1f} s: wobble after stop  cubic {residual(c3, Tm):5.2f} deg   quintic {residual(c5, Tm):5.2f} deg")

Output (run with python3 quintic.py)

Part 1 - quintic coefficients, 30 -> 120 deg in 1.5 s, rest to rest
  6x6 solve: [30.0, 0.0, 0.0, 266.667, -266.667, 71.111]
  formula  : [30.0, 0, 0, 266.667, -266.667, 71.111]

Part 2 - cubic vs quintic, same move
            peak|v|   peak|a|   a(0)    a(T)    jerk at t=0
cubic        90.0     240.0   240.0  -240.0     -320.0
quintic     112.5     230.9     0.0     0.0     1600.0
formulas: cubic v 1.5d/T = 90.0, a 6d/T^2 = 240.0; quintic v 15d/(8T) = 112.5, a 10d/(sqrt(3)T^2) = 230.9

Part 3 - shortest T with v <= 100 deg/s and a <= 200 deg/s^2
   20.0 deg: cubic 0.775 s, quintic 0.760 s
   90.0 deg: cubic 1.643 s, quintic 1.688 s
  150.0 deg: cubic 2.250 s, quintic 2.812 s

Part 4 - springy arm: tip lags the joint like a mass on a spring (f = 3 Hz, damping 2%)
  T = 0.6 s: wobble after stop  cubic  6.85 deg   quintic  0.97 deg
  T = 1.0 s: wobble after stop  cubic  2.56 deg   quintic  0.47 deg
  T = 1.5 s: wobble after stop  cubic  0.30 deg   quintic  0.36 deg

Try it: switch between linear, cubic and quintic. Compare the acceleration plots at t = 0 and t = T, and the height of the velocity peak.

Interactive · Motion profile comparator

Quintic polynomial rest-to-rest trajectoryJerk and smooth motion comparisonMulti-joint synchronization shared progress

Overlay linear, cubic, and quintic rest-to-rest profiles. Linear has velocity steps (harsh); cubic zeros velocity; quintic also zeros acceleration — lowest jerk among the three.

Position q(t)

Velocity q̇(t)

Acceleration q̈(t)

Jerk q⃛(t)

Profilepeak |q̇|peak |q̈|peak |jerk|
Linear36.00.0∞ (edge)
Cubic54.086.469.1
Quintic67.583.1345.6

Educational 1-DOF overlays — not full multi-DOF dynamics. Linear jerk shown as edge impulse in table.

Real world

Quintic motion in the real world

  • Long, light arms: a space arm like Canadarm2 is a very long, slender structure; the longer and lighter a link, the more easily a jerky start or stop sets it shaking.
  • Pick-and-place of fragile or liquid items: low-jerk profiles stop cups, vials and trays from sloshing or tipping.
  • Camera robots and gimbals: smooth acceleration at the ends avoids visible shake at the start and end of a shot.
  • Motor drives: many servo drives offer S-curve or jerk-limited profiles for the same reason — less vibration and wear.
A long white robotic arm folded at its elbow on the International Space Station, the blue Earth below
Canadarm2 on the International Space Station. A very long, light arm is like the wobbly ruler on a giant scale: smooth starts and stops matter.Photo: NASA · Public domain · Source: Wikimedia Commons · resized, converted to WebP

Bench build

Bench build: the wobbly ruler test

Tape a light 15 cm plastic ruler firmly to a servo horn, sticking straight out, and add a small blob of Blu-Tack at the tip to make the wobble easier to see. flex_arm_cubic_vs_quintic.ino swings it between 60° and 120° in 0.6 s. D2 runs a cubic, D3 a quintic. Film both with your phone’s slow-motion mode and count the wobbles after the servo stops.

Wiring
FromToNote
Servo signalD9—
Servo + and −Separate 5–6 V supplyNever the Arduino 5 V pin
Supply −Arduino GNDCommon ground
ButtonsD2 ↔ GND, D3 ↔ GNDINPUT_PULLUP
  1. Mount

    Clamp the servo to a heavy block at the table edge so the ruler swings freely in the air.

  2. Cubic (D2)

    Press D2. Film in slow motion. Count how many visible wobbles the tip makes after the servo stops.

  3. Quintic (D3)

    Press D3 and film again. Compare the counts and the size of the first wobble.

  4. Change T_MOVE

    Try 0.4 s and 1.5 s. At which T does the difference almost vanish? Compare with Part 4 of the Python output.

  5. Change the arm

    Move the Blu-Tack closer to the servo (stiffer, faster wobble). Does the cubic still wobble as much?

flex_arm_cubic_vs_quintic.ino · Arduino C++
// flex_arm_cubic_vs_quintic.ino — Stage 9, Lesson 8
// A springy arm: a light 15 cm plastic ruler taped to a servo horn wobbles after quick moves.
//   D2: cubic   move 60° -> 120° (or back) in T_MOVE seconds
//   D3: quintic move, same angles, same time
// Watch (or film in slow motion) how much the ruler tip wobbles after the servo stops.
//   cubic   s(u) = 3u^2 - 2u^3
//   quintic s(u) = 10u^3 - 15u^4 + 6u^5      (u = t / T, Modern Robotics 9.2.2.1)
// Wiring: servo signal -> D9; servo + and - -> SEPARATE 5–6 V supply; supply GND -> Arduino GND.
//         Buttons D2, D3 -> GND (INPUT_PULLUP). Never power the servo from an Arduino pin.

#include <Servo.h>

const float A_DEG = 60.0, B_DEG = 120.0;
const float T_MOVE = 0.6;               // short on purpose: wobble shows best on quick moves
const unsigned long DT_MS = 10;         // 100 Hz updates

Servo s;
float current = A_DEG;

float sCubic(float u)   { return u * u * (3 - 2 * u); }
float sQuintic(float u) { return u * u * u * (10 + u * (-15 + 6 * u)); }

void writeDeg(float d) {
  s.writeMicroseconds(544 + (long)(d * (2400 - 544) / 180.0 + 0.5));
}

void move(bool quintic) {
  float q0 = current, qf = (current < 90) ? B_DEG : A_DEG;
  Serial.print(quintic ? F("QUINTIC ") : F("CUBIC   "));
  Serial.print(q0, 0); Serial.print(F(" -> ")); Serial.println(qf, 0);
  unsigned long t0 = millis();
  while (true) {
    float u = (millis() - t0) / 1000.0 / T_MOVE;
    if (u > 1) u = 1;
    float sv = quintic ? sQuintic(u) : sCubic(u);
    writeDeg(q0 + sv * (qf - q0));
    if (u >= 1) break;
    delay(DT_MS);
  }
  current = qf;
  delay(1500);                          // let the wobble die before the next press
}

void setup() {
  Serial.begin(115200);
  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  s.attach(9);
  writeDeg(current);
  Serial.println(F("D2 = cubic, D3 = quintic"));
}

void loop() {
  if (digitalRead(2) == LOW) move(false);
  if (digitalRead(3) == LOW) move(true);
}
Five rulers lying in parallel: two stainless steel, two clear plastic and one wooden, with metric and imperial scales
Ordinary school rulers. A clear plastic one taped to a servo horn makes a perfect flexible “link” for the wobble test; a steel one is too heavy for a small servo.Photo: Estabiano · CC0 · Source: Wikimedia Commons · resized, converted to WebP

Debugging

Debugging quintic trajectories

SymptomLikely causeFix
Quintic overshoots the goalWrong sign on a4 (should be −15)Check s(1) = 10 − 15 + 6 = 1
Quintic trips the speed limit, cubic did notQuintic peak speed is 1.875·d/TLengthen T: T ≥ 15d/(8·v_lim)
No visible difference on the benchMove too slow for the arm’s wobble periodShorten T_MOVE or make the arm floppier
Both profiles wobble a lotServo’s own controller overshoots, or arm too heavyLighter arm; check the servo is not overloaded
6×6 solve gives huge numbersT in milliseconds instead of secondsUse seconds

Think

Think about it

Think

Your arm carries a cup of chai and its speed limit is the active limit for a long 150° move. Your friend wants to switch from cubic to quintic “to be safer”. What will happen to the move time, and is the switch still worth it?

Experiment

Try it in the lab

About 35 minutes with quintic.py and the browser lab; the ruler test takes 10 more minutes and is great fun.

Experiment

When does quintic win?

You will need

  • `quintic.py` and Python 3
  • Graph paper or a spreadsheet
  • Motion profile comparator (in your browser)
  • Optional: Arduino UNO, one servo, separate 5–6 V supply, 2 buttons, a 15 cm plastic ruler, Blu-Tack, a phone with slow motion

Steps

  1. Run `quintic.py`. Plot the acceleration of the cubic and the quintic on the same axes for T = 1.5 s.
  2. Check by hand that s(u) = 10u³ − 15u⁴ + 6u⁵ gives s(0) = 0, s(1) = 1 and s(0.5) = 0.5.
  3. In Part 4, change the spring to 1.5 Hz (a floppier arm). Which T values now show a big difference?
  4. Add T = 0.4 s to Part 4. How large is the cubic’s wobble now?
  5. In Part 3, find the move size where cubic and quintic need the same T.
  6. Optional: run the ruler test and compare your wobble counts with the simulation.

Interactive · Motion profile comparator

Quintic polynomial rest-to-rest trajectoryJerk and smooth motion comparisonMulti-joint synchronization shared progress

Overlay linear, cubic, and quintic rest-to-rest profiles. Linear has velocity steps (harsh); cubic zeros velocity; quintic also zeros acceleration — lowest jerk among the three.

Position q(t)

Velocity q̇(t)

Acceleration q̈(t)

Jerk q⃛(t)

Profilepeak |q̇|peak |q̈|peak |jerk|
Linear36.00.0∞ (edge)
Cubic54.086.469.1
Quintic67.583.1345.6

Educational 1-DOF overlays — not full multi-DOF dynamics. Linear jerk shown as edge impulse in table.

The quintic removes the acceleration steps at the ends and greatly reduces wobble on quick moves of a flexible arm. On slow moves the benefit fades. Its higher peak speed means it needs more time when the speed limit is active.

Knowledge check

Check your understanding

Knowledge check

1. How many coefficients does a quintic polynomial have?

2. Which two extra conditions does a rest-to-rest quintic add to the cubic’s four?

3. Why can a cubic make a light robot link vibrate?

4. For the same move and T, the quintic’s peak speed compared with the cubic’s is…

5. s(u) = 10u³ − 15u⁴ + 6u⁵. What is s(0.5)?

6. In Part 4, at T = 0.6 s, the wobble after a cubic was about 6.85° and after a quintic about…

7. Your speed limit is the active limit for a long move. Switching from cubic to quintic will make T…

Mini assignment

Hands-on assignment

Mini assignment

Choose your profile with evidence

For one joint of your arm, decide between cubic and quintic using numbers from simulation and (optionally) the ruler test.

Deliverables

  • Write `plan_quintic(q0, qf, T)` returning the six coefficients, and `t_min_quintic(d, v_lim, a_lim)`.
  • For three moves of your own (small, medium, large), make a table: T_min for cubic and quintic, and which limit decides.
  • Use Part 4 with your guess of your arm’s wobble frequency (count wobbles per second on a slow-motion video) and report the residual wobble for both profiles.
  • A recommendation in 3–4 sentences: which profile you will use for your arm, and when you would switch.
  • Optional: slow-motion clips (or wobble counts) for cubic and quintic on the ruler test.

Do this, step by step

  1. Run python3 quintic.py and check Part 1: the six coefficients from the 6×6 solve match the closed form.
  2. From Part 2, write the peak speed and peak acceleration of the cubic and the quintic for the same move. Which is higher?
  3. In Part 3, write the shortest T for both profiles. Why is the quintic slower for the same limits?
  4. In Part 4, change the arm’s wobble frequency to your own guess and compare the residual wobble for both.
  5. Optional bench: run flex_arm_cubic_vs_quintic.ino with a ruler on the horn, film in slow motion and count wobbles after each stop.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. A quintic has 6 coefficients: position, speed and acceleration at both ends.
  2. For rest-to-rest moves, the quintic’s peak speed is 1.875·d/T, which is 25% higher than the cubic’s 1.5·d/T.
  3. Count wobbles per second on a slow-motion video to estimate the frequency; one full back-and-forth is one wobble.

Challenges (pick by level)

  • 🟢 Easy: For a 60° move in 2 s, compute the peak speed of a cubic and a quintic.
  • 🟢 Easy: List three objects at home that wobble after being moved quickly.
  • 🟡 Medium: Plot the jerk of the cubic and the quintic over time using the poly() function.
  • 🟠 Hard-ish: Use quintic_coeffs with non-zero end velocity to join two quintic segments through a waypoint, keeping acceleration continuous.
  • 🔴 Hard: In Part 4, search over T from 0.3 to 2.0 s and plot residual wobble for both profiles. Explain the dips you see.
  • ⚫ Research: Read the S-curve paragraph in *Modern Robotics* §9.2.2.3 and list its seven phases in your own words.

Next step

So far every trajectory lived in joint space. In Lesson 9 — Cartesian Trajectories you will move the tool itself along straight lines and arcs, sampling IK along the way and watching for the warnings you met in Lesson 4.

Look back: Lesson 7: Cubic Polynomial Trajectories. Next: Lesson 9: Cartesian Trajectories. Related: Stage 12 — Simulation & Digital Twin.

Sources

Self-attested for now — keep artifacts in your lab notebook.

Complete

Today's goal

0 of 1 done today

0-day streak
Daily goal:lessons, quizzes or assignments a day

Mark this lesson complete when you have worked the experiment, knowledge check, and assignment (even roughly).

Stuck somewhere?

Ask a doubt about this lesson

Saved with this lesson's id and your email so we can reply. Privacy · or email contact@aihubstation.in