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.

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:
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) = accfFor 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:
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 | |
|---|---|---|
| Conditions | 4: position and velocity at both ends | 6: 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/end | Infinite (acceleration step) | Finite |
| Peak speed | 1.5·d/T | 1.875·d/T (25% higher) |
| Peak acceleration | 6·d/T² | ≈ 5.77·d/T² (slightly lower, but in the middle of the speed-up) |
| Good for | Light, stiff arms; simple code | Flexible 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
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)
| Profile | peak |q̇| | peak |q̈| | peak |jerk| |
|---|---|---|---|
| Linear | 36.0 | 0.0 | ∞ (edge) |
| Cubic | 54.0 | 86.4 | 69.1 |
| Quintic | 67.5 | 83.1 | 345.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 — 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 degTry 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
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)
| Profile | peak |q̇| | peak |q̈| | peak |jerk| |
|---|---|---|---|
| Linear | 36.0 | 0.0 | ∞ (edge) |
| Cubic | 54.0 | 86.4 | 69.1 |
| Quintic | 67.5 | 83.1 | 345.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.

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.
| From | To | Note |
|---|---|---|
| Servo signal | D9 | — |
| Servo + and − | Separate 5–6 V supply | Never the Arduino 5 V pin |
| Supply − | Arduino GND | Common ground |
| Buttons | D2 ↔ GND, D3 ↔ GND | INPUT_PULLUP |
Mount
Clamp the servo to a heavy block at the table edge so the ruler swings freely in the air.
Cubic (D2)
Press D2. Film in slow motion. Count how many visible wobbles the tip makes after the servo stops.
Quintic (D3)
Press D3 and film again. Compare the counts and the size of the first wobble.
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.
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 — 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);
}
Debugging
Debugging quintic trajectories
| Symptom | Likely cause | Fix |
|---|---|---|
| Quintic overshoots the goal | Wrong sign on a4 (should be −15) | Check s(1) = 10 − 15 + 6 = 1 |
| Quintic trips the speed limit, cubic did not | Quintic peak speed is 1.875·d/T | Lengthen T: T ≥ 15d/(8·v_lim) |
| No visible difference on the bench | Move too slow for the arm’s wobble period | Shorten T_MOVE or make the arm floppier |
| Both profiles wobble a lot | Servo’s own controller overshoots, or arm too heavy | Lighter arm; check the servo is not overloaded |
| 6×6 solve gives huge numbers | T in milliseconds instead of seconds | Use 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
- Run `quintic.py`. Plot the acceleration of the cubic and the quintic on the same axes for T = 1.5 s.
- Check by hand that s(u) = 10u³ − 15u⁴ + 6u⁵ gives s(0) = 0, s(1) = 1 and s(0.5) = 0.5.
- In Part 4, change the spring to 1.5 Hz (a floppier arm). Which T values now show a big difference?
- Add T = 0.4 s to Part 4. How large is the cubic’s wobble now?
- In Part 3, find the move size where cubic and quintic need the same T.
- Optional: run the ruler test and compare your wobble counts with the simulation.
Interactive · Motion profile comparator
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)
| Profile | peak |q̇| | peak |q̈| | peak |jerk| |
|---|---|---|---|
| Linear | 36.0 | 0.0 | ∞ (edge) |
| Cubic | 54.0 | 86.4 | 69.1 |
| Quintic | 67.5 | 83.1 | 345.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
- Run
python3 quintic.pyand check Part 1: the six coefficients from the 6×6 solve match the closed form. - From Part 2, write the peak speed and peak acceleration of the cubic and the quintic for the same move. Which is higher?
- In Part 3, write the shortest T for both profiles. Why is the quintic slower for the same limits?
- In Part 4, change the arm’s wobble frequency to your own guess and compare the residual wobble for both.
- Optional bench: run
flex_arm_cubic_vs_quintic.inowith a ruler on the horn, film in slow motion and count wobbles after each stop.
Stuck? Hints, one at a time
- A quintic has 6 coefficients: position, speed and acceleration at both ends.
- 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.
- 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_coeffswith 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
- Modern Robotics (Lynch & Park) — book home page and free PDF — §9.2.2.1 fifth-order time scaling (cubic’s acceleration jump → infinite jerk → vibration; six terminal constraints; smoother motion with higher maximum velocity; Fig. 9.4 peaks 15/(8T) and 10/(√3·T²)); §9.2.2.3 S-curve.
- Modern Robotics video — 9.1 and 9.2 Point-to-Point Trajectories — companion lecture.
- Wikipedia — Jerk (physics) — jerk as the rate of change of acceleration.
- Wikipedia — Mobile Servicing System (Canadarm2) — the space station’s robotic arm.
- Photos: Wikimedia Commons (credits under each image).
Self-attested for now — keep artifacts in your lab notebook.
Complete
Today's goal
0 of 1 done today
Mark this lesson complete when you have worked the experiment, knowledge check, and assignment (even roughly).