Skip to content

Trajectory: closed-form quintic polynomial reference generation

This tool calculates a smooth, continuous path to move any number of robotic joints from an initial position to a target destination within a set time frame. By using a fifth-degree equation, it guarantees that the movement starts and stops seamlessly, preventing sudden jerks or mechanical vibrations at the endpoints.

The rate limiter bounds how fast a command can change and the CBF filter bounds where it can go, but neither generates a reference to track in the first place. Something has to say "go from configuration A to configuration B over T seconds, smoothly". Trajectory is that something: a closed-form, minimum-jerk-continuous path between two configurations, for any number of joints at once.

The generator is a quintic polynomial: fifth-degree in time, chosen so that position, velocity, and acceleration all start at their initial values and end at their final values with no jump. That is the smallest degree that can do it, and the smallest degree that makes the resulting motion feel right — no jerk at the endpoints, no overshoot, no oscillation.

The two configurations

A 6-joint arm has to move from its current joint configuration to a target one, in 2 seconds.

The arm is at rest before and after the move: no residual velocity, no residual acceleration.

1. Generate the trajectory

import numpy as np
from dense_armor.utility.control.trajectory import quintic_trajectory

q0 = np.array([0.0, 0.5, -0.3, 0.1, 0.0, 0.0])
qf = np.array([0.5, 1.0, -0.1, 0.2, 0.0, 0.0])
t, q, v, a = quintic_trajectory(q0=q0, qf=qf, T=2.0)
print(q[0].round(3), q[-1].round(3), np.abs(v).max(axis=0).round(3))

[ 0.   0.5 -0.3  0.1  0.   0. ] [ 0.5  1.  -0.1  0.2  0.   0. ] [0.469 0.469 0.187 0.094 0.    0.   ]

The path starts exactly at q0, ends exactly at qf, and the largest joint speed is 0.469 rad/s.

t is a time vector; q, v, a are the position, velocity and acceleration profiles, all with shape (len(t), 6). Every joint gets its own independent polynomial over the same real time T. The function returns a fine-grained grid of samples so that any downstream consumer (a controller, a plot, a replay) has the full profile.

2. The polynomial, symbol by symbol

The position at time s ∈ [0, T] is a fifth-degree polynomial:

q(s) = c₀ + c₁ s + c₂ s² + c₃ s³ + c₄ s⁴ + c₅ s⁵

The six coefficients are determined by six boundary conditions — three at s = 0 and three at s = T:

q(0) = q0,   q'(0) = v0,   q''(0) = a0
q(T) = qf,   q'(T) = vf,   q''(T) = af

For the default of "start and end at rest", v0 = vf = a0 = af = 0. Solving the six equations gives the six coefficients in closed form; there is no iteration, no optimization, no trajectory library.

Hand case. Single joint, q0 = 0, qf = 1, T = 2, start and end at rest. The coefficients come out as c₀ = 0, c₁ = 0, c₂ = 0, c₃ = 1.25, c₄ = −0.9375, c₅ = 0.1875. The position at s = 1 (halfway in time) is 1.25 − 0.9375 + 0.1875 = 0.5 — exactly halfway in position. That symmetry holds for any q0, qf, T with rest at both ends, and it is one of the properties that makes the quintic pleasant: the motion is centred on the move's midpoint.

3. Chaining segments

Pass v0 and vf to start or end already moving instead of at rest. This is what makes it possible to chain several segments without the robot stopping at every intermediate waypoint:

import numpy as np
from dense_armor.utility.control.trajectory import quintic_trajectory

q0, qf = np.array([0.0, 0.5]), np.array([0.5, 1.0])
q_mid = 0.5 * (q0 + qf)
t1, q1, v1, a1 = quintic_trajectory(q0, q_mid, T=1.0)
t2, q2, v2, a2 = quintic_trajectory(q_mid, qf, T=1.0, v0=v1[-1], a0=a1[-1])
print(q1[-1].round(3), q2[0].round(3), v1[-1].round(3), v2[0].round(3))

[0.25 0.75] [0.25 0.75] [0. 0.] [0. 0.]

Two quintic pieces joined at the midpoint: same position (0.25, 0.75) and same velocity (0) at the joint.

The end velocity of the first segment becomes the start velocity of the second, so the velocity profile is continuous across the join. The acceleration too. The result is a piecewise-quintic trajectory with C² continuity everywhere, which is more than enough for a low-level controller to track smoothly.

4. Any number of joints

The function is fully vectorised: q0 and qf can be scalars, or 1-D arrays of any length, or 2-D arrays for a batched call. Every joint uses the same time grid and its own independent polynomial, so no coordination between joints is needed. If you need joints to move in a coordinated way (arrive at the same time, or maintain a fixed relative phase), that is a downstream concern: you feed each joint's trajectory into the kinematic controller, which handles the coupling at the velocity level.

5. Downstream

The output is the reference. Two standard consumers:

import numpy as np
from dense_armor.utility.control.trajectory import quintic_trajectory
from dense_armor.utility.control.kinematic_controller import kinematic_tracking_controller

t, q, v, a = quintic_trajectory(np.zeros(2), np.array([0.5, 1.0]), T=2.0)
dt, qa = t[1] - t[0], np.array([0.05, -0.05])
for i in range(len(t)):
    qa = qa + dt * kinematic_tracking_controller(qa, q[i], v[i], kp=5.0)
print(np.abs(qa - q[-1]).round(4))

[0.0007 0.0014]

The kinematic controller follows a 2-joint quintic path starting 0.05 rad off; at the end the error is below 0.0015 rad.

The trajectory says where to go; the kinematic controller says how to get there on a single-integrator plant; the rate limiter and CBF filter keep the resulting command safe in speed and space.

API reference

trajectory

utility/trajectory.py

Closed-form, no-training point-to-point trajectory generator -- rate_limiter.py bounds how fast a command can change and cbf_filter.py bounds where it can go, but neither generates a REFERENCE to track in the first place. This module fills that gap with the simplest real, universal piece: a minimum-jerk-continuous quintic polynomial between two points.

Scoped down deliberately from two real papers (both read in full before writing any code): Lozer, Scalera, Boscariol & Gasparetto, "Planning optimal minimum-jerk trajectories for redundant robots" (Robotics and Autonomous Systems, Elsevier) does multi-stage optimization with a full dynamic model on a real 7-DoF Franka Panda; Fried & Paternain, "A Bi-Level Optimization Approach to Joint Trajectory Optimization for Redundant Manipulators" (arXiv:2412.07859) does a convex inner/primal- dual outer optimization validated on a real UR10e. Both need a full kinematic/dynamic model (URDF parsing, Jacobians, torque limits) -- not implemented here, a real, separate, much larger undertaking.

THEORY: the classic quintic (5th-order) polynomial trajectory (see e.g. Craig, "Introduction to Robotics", or Piazzi & Visioli 2000) is the unique degree-5 polynomial per joint satisfying 6 real boundary conditions (position, velocity, acceleration at t=0 and t=T) -- solved here directly from the 6x6 linear system, not copied from a memorized formula, so a transcription error would fail the boundary-condition test rather than silently produce a wrong trajectory.

UNIVERSAL in the sense that matters for this stack: works for any number of joints at once (any robot) since each joint's polynomial is independent; needs no URDF, no kinematics, no dynamics, no robot connection. Composes directly with rate_limiter/cbf_filter, which already own rate-of-change and spatial safety -- this generator does not need to worry about either.

VALIDATED, honestly, on TWO independent real physical domains (real LeRobot robot-arm joint data), promoted from Dense-Evolution-Discovery after both checked out (see docs/quintic_trajectory_planner.md there). A first validation attempt (real episode start/end frames as q0/qf) was thrown out as degenerate: a real pick-and-place task often returns close to its own starting configuration, giving several joints almost nothing to do and nothing meaningful to compare. Fixed by comparing each real joint's own min-to-max excursion within the episode instead: - SO-101 (single 6-DoF arm, real 30Hz): 6 real joint excursions checked. - ALOHA (bimanual, 14-DoF, real 50Hz -- a genuinely different real robot): 14 real joint excursions checked. Real result, all 20/20 real excursions: the quintic's peak velocity is always LOWER than the real recorded peak velocity for the same start, end, and real elapsed duration (ratio 0.05-0.62, mean 0.26). Expected, not a bug: the quintic is the smoothest possible path between two points, so it needs less peak speed than a real (teleoperated, not necessarily efficient) trajectory covering the same net real displacement in the same real time.

HONEST SCOPE: single-segment point-to-point only. Chaining several quintic segments across many waypoints is a real, natural next step, not implemented here.

quintic_trajectory 

quintic_trajectory(
    q0,
    qf,
    T: float,
    v0=None,
    a0=None,
    vf=None,
    af=None,
    n_samples: int = 100,
)

Closed-form quintic point-to-point trajectory, any number of DOF.

Parameters

q0, qf : array-like, shape (n_dof,) Start and end position for each joint. T : float Total real trajectory duration. v0, a0, vf, af : array-like, shape (n_dof,), optional Start/end velocity and acceleration per joint (default 0 -- the standard "start and end at rest" case). n_samples : int Number of real time samples to return.

Returns

t : ndarray, shape (n_samples,) q, v, a : ndarray, shape (n_samples, n_dof) Position, velocity, acceleration at each sampled time.

Source code in dense_armor/utility/control/trajectory.py 
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
def quintic_trajectory(q0, qf, T: float, v0=None, a0=None, vf=None, af=None, n_samples: int = 100):
    """Closed-form quintic point-to-point trajectory, any number of DOF.

    Parameters
    ----------
    q0, qf : array-like, shape (n_dof,)
        Start and end position for each joint.
    T : float
        Total real trajectory duration.
    v0, a0, vf, af : array-like, shape (n_dof,), optional
        Start/end velocity and acceleration per joint (default 0 -- the
        standard "start and end at rest" case).
    n_samples : int
        Number of real time samples to return.

    Returns
    -------
    t : ndarray, shape (n_samples,)
    q, v, a : ndarray, shape (n_samples, n_dof)
        Position, velocity, acceleration at each sampled time.
    """
    q0 = np.atleast_1d(np.asarray(q0, dtype=float))
    qf = np.atleast_1d(np.asarray(qf, dtype=float))
    n_dof = q0.shape[0]
    assert qf.shape[0] == n_dof, "q0 and qf must have the same number of joints"
    v0 = np.zeros(n_dof) if v0 is None else np.atleast_1d(np.asarray(v0, dtype=float))
    a0 = np.zeros(n_dof) if a0 is None else np.atleast_1d(np.asarray(a0, dtype=float))
    vf = np.zeros(n_dof) if vf is None else np.atleast_1d(np.asarray(vf, dtype=float))
    af = np.zeros(n_dof) if af is None else np.atleast_1d(np.asarray(af, dtype=float))
    assert T > 0, "T must be positive"

    t = np.linspace(0.0, T, n_samples)

    boundary_matrix = np.array([
        [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],
    ])
    rhs = np.stack([q0, v0, a0, qf, vf, af], axis=1)          # (n_dof, 6)
    coeffs = np.linalg.solve(boundary_matrix, rhs.T)           # (6, n_dof)

    powers_q = t[:, None] ** np.arange(6)[None, :]                                    # (n_samples, 6)
    powers_v = np.concatenate([np.zeros((n_samples, 1)),
                                np.arange(1, 6)[None, :] * t[:, None] ** np.arange(0, 5)[None, :]], axis=1)
    powers_a = np.concatenate([np.zeros((n_samples, 2)),
                                (np.arange(2, 6) * np.arange(1, 5))[None, :] * t[:, None] ** np.arange(0, 4)[None, :]], axis=1)

    q = powers_q @ coeffs
    v = powers_v @ coeffs
    a = powers_a @ coeffs
    return t, q, v, a

Details

Scoped down from two papers proposing much larger URDF- and dynamics-aware trajectory optimizers (Lozer, Scalera, Boscariol & Gasparetto, Robotics and Autonomous Systems; Fried & Paternain, arXiv:2412.07859 — A Bi-Level Optimization Approach to Joint Trajectory Optimization, Section V uses a polynomial basis of degree five for the joint trajectory and a time-scaling variable θ on top; the closed form here uses the quintic only, no inner optimization). Both were read in full before writing this. They solve the problem the same way in principle — generate a smooth trajectory from a boundary-value problem — but they integrate the robot's real dynamics into the generation, which requires the full mass matrix and Coriolis terms. That is the scope of the passivity + singularity-CBF controller, not of this module.

The module was promoted from Dense-Evolution-Discovery after validation on two independent real physical domains (SO-101, ALOHA, 20 real joint excursions): the quintic's peak velocity is always lower than the real recorded peak velocity for the same start, end, and real elapsed duration. This is expected — the quintic is the smoothest possible point-to-point path, so it uses less peak velocity than a human operator's motion for the same total displacement and time.

Single-segment only. Chaining several segments across many waypoints is a real next step, not implemented in the generator itself. The manual chaining in section 3 above is the workaround and gives the same C² continuity.

See also: Rate limiter and CBF filter — this module generates a reference to follow; those two keep whatever follows it safe in speed and space.