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:

QuantityPeak valueWhere
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

FieldSymbolDim
position3
orientation4
linear velocity3
angular velocity3
linear acceleration3

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, a

Sim-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