Problems · Problem 3 · Kinematics · Easy

Polynomial trajectories

Solve for the quintic that meets position, velocity and acceleration at both ends, then chain quintics through waypoints.

Boundary conditionsQuintic polynomialsVia points

Builds on Linear systems: calibrate a camera, from the free Foundations.

Write and run this problem in the simulator with Pro

What it computes

A trajectory says where each joint should be at every instant. Controllers need smooth setpoints: a jump in velocity or acceleration asks the motors for infinite torque. A quintic polynomial per joint is the standard tool, because it has enough coefficients to fix position, velocity and acceleration at both ends of a move.

Six conditions, six unknowns

q(t) = c_0 + c_1 t + c_2 t^2 + c_3 t^3 + c_4 t^4 + c_5 t^5

Ask for $q, \dot q, \ddot q$ to equal $(q_0, v_0, a_0)$ at $t = 0$ and $(q_1, v_1, a_1)$ at $t = T$ . Each condition is linear in the $c_k$ , so together they form a $6 \times 6$ system $M c = b$ :

\begin{bmatrix} 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 & 2T & 3T^2 & 4T^3 & 5T^4 \\ 0 & 0 & 2 & 6T & 12T^2 & 20T^3 \end{bmatrix} \begin{bmatrix} c_0 \\ c_1 \\ c_2 \\ c_3 \\ c_4 \\ c_5 \end{bmatrix} = \begin{bmatrix} q_0 \\ v_0 \\ a_0 \\ q_1 \\ v_1 \\ a_1 \end{bmatrix}

np.linalg.solve(M, b) handles every joint at once: if $b$ is $6 \times n$ (one column per joint), so is $c$ .

Through waypoints

To pass through waypoints without stopping, give each interior waypoint a velocity and use it as $v_1$ of one segment and $v_0$ of the next. Position and velocity then stay continuous. The program uses the average of the neighbouring segments' slopes.

See it in context

Trajectory Generation, steps 1–5, builds cubic, quintic and via-point trajectories one at a time.

Your task

Implement quintic_coeffs(q0, v0, a0, q1, v1, a1, T) and quintic_eval(c, t), which returns (q, qd, qdd). The program streams four waypoints through them at 100 Hz and plots the commanded velocities.

What it computes

Six conditions, six unknowns

q(t) = c_0 + c_1 t + c_2 t^2 + c_3 t^3 + c_4 t^4 + c_5 t^5

Ask for

q, \dot q, \ddot q

to equal

(q_0, v_0, a_0)

t = 0

and

(q_1, v_1, a_1)

t = T

. Each condition is linear in the

c_k

, so together they form a

6 \times 6

system

M c = b

\begin{bmatrix} 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 & 2T & 3T^2 & 4T^3 & 5T^4 \\ 0 & 0 & 2 & 6T & 12T^2 & 20T^3 \end{bmatrix} \begin{bmatrix} c_0 \\ c_1 \\ c_2 \\ c_3 \\ c_4 \\ c_5 \end{bmatrix} = \begin{bmatrix} q_0 \\ v_0 \\ a_0 \\ q_1 \\ v_1 \\ a_1 \end{bmatrix}

np.linalg.solve(M, b) handles every joint at once: if

b

6 \times n

(one column per joint), so is

c

Polynomial trajectories

What it computes

Six conditions, six unknowns

Through waypoints

See it in context

Your task

This problem is part of CrackRobotics Pro

Polynomial trajectories

What it computes

Six conditions, six unknowns

Through waypoints

See it in context

Your task

This problem is part of CrackRobotics Pro