Overview
- a quintic polynomial is a 5th order time law with 6 coefficients, enough to fix position, velocity and acceleration at both ends
- the rest to rest form is the minimum jerk reach, close to how humans move their arms
- its acceleration is continuous, but the jerk still steps at the start and the end
A cubic polynomial can match position and velocity at both ends but leaves acceleration free, so the commanded torque jumps at the start. Adding two more coefficients lets us also fix the two end accelerations.
Mathematical Formulation
Polynomial and boundary conditions
Six conditions give six unknowns:
Linear system
Closed form coefficients
With :
Rest to rest (normalised)
With , and :
Its derivatives with respect to real time:
| Quantity | Peak value | Where |
|---|---|---|
| velocity | ||
| acceleration | ||
| jerk | and |
The jerk is before the move and right after it starts, so the jerk steps at both ends.
Choosing T from limits
Invert the table so no limit is broken:
Minimum jerk property
Among all motions from rest to rest in time , the rest to rest quintic minimises
which is the minimum jerk model of human reaching (Flash and Hogan, 1985).
Use in Cartesian space
Compute once and share it:
For several via points, run one quintic per segment and pass the end of one segment as the start of the next.
Setpoint vector layout
| Field | Symbol | Dim |
|---|---|---|
| position | 3 | |
| orientation | 4 | |
| linear velocity | 3 | |
| angular velocity | 3 | |
| linear acceleration | 3 |
Algorithmic / Architectural Flow
flowchart LR A["Boundary state s0 v0 a0 and s1 v1 a1"] --> B["Pick T from v, a, j limits"] B --> C["Solve c0 to c5"] C --> D["Sample s(t) and derivatives each tick"] D --> E["sigma(t)"] E --> F["Position lerp and SLERP"] F --> G["IK or Cartesian controller"]
import numpy as np
def quintic(s0, v0, a0, s1, v1, a1, T):
h = s1 - s0
c3 = (20*h - (8*v1 + 12*v0)*T - (3*a0 - a1)*T**2) / (2*T**3)
c4 = (-30*h + (14*v1 + 16*v0)*T + (3*a0 - 2*a1)*T**2) / (2*T**4)
c5 = (12*h - 6*(v1 + v0)*T + (a1 - a0)*T**2) / (2*T**5)
return np.array([s0, v0, a0/2, c3, c4, c5])
def eval_quintic(c, t):
s = np.polyval(c[::-1], t)
v = np.polyval(np.polyder(c[::-1]), t)
a = np.polyval(np.polyder(c[::-1], 2), t)
return s, v, aSim-to-Real & Hardware Failure Modes
Things that break on hardware:
- jerk steps at the ends: the jump is small for slow reaches but can kick a light wrist on fast ones, use an S-curve or a 7th order polynomial if the end jerk matters
- overshoot with non zero boundary velocity: when or is large compared to , can leave , so goes outside and SLERP extrapolates past the goal orientation
- stale start state on replanning: replanning from a measured state that lags the commanded one puts a step into the setpoint, start the new segment from the last commanded , not the noisy measured one
- joint limits: like any Cartesian profile, a safe in Cartesian space can still exceed joint speed near singularities, check along the path
- high order through many points: one polynomial through many via points oscillates between them, always use one quintic per segment
Cross-Links
- the jerk bounded alternative is the S-Curve Trajectory Profile
- the setpoints feed a controller such as Cartesian Based Control
- orientation uses Spherical Linear Interpolation (SLERP) with the same