Introduction
In robot kinematics, a joint path is a sequence of positions for one or more joints. A joint trajectory is the time function interpolating these positions.
This post examines generating joint trajectories with cubic splines.
Say we have a robotic arm with one revolute joint, and we want to rotate its joint position \(Q\) from \(0\) to \(90\) degrees.
$$ \begin{aligned} Q_{init} &= 0 \newline Q_{final} &= \pi/2 \newline \end{aligned} $$
Figure: The joint start and goal
We don't care how long it takes, but the joint must start from rest and and end at rest.
$$ \begin{aligned} V_{init} &= 0 \newline V_{final} &= 0 \newline \end{aligned} $$
Initial Solution
We can satisfy these constraints by interpolating the joint position, velocity, and acceleration with a cubic spline,
$$ \begin{aligned} Q(t) &= At^3 + Bt^2 + Ct + D &&\text{// Position} \newline V(t) &= 3At^2 + 2Bt + C &&\text{// Velocity} \newline A(t) &= 6At + 2B &&\text{// Acceleration} \newline \end{aligned} $$
where \(t\) is the time since the movement started.
We start by finding the coefficients \(A\), \(B\), \(C\), and \(D\).
$$ \begin{aligned} Duration &= \textit{(To be Determined)} \newline Displacement &= Q_{final} - Q_{init} \newline A &= \frac{(2 \cdot -Displacement / Duration + V_{init} + V_{final})}{Duration^2} \newline B &= \frac{(3 \cdot Displacement / Duration - 2 \cdot V_{init} - V_{final})}{Duration} \newline C &= V_{init} \newline D &= Q_{init} \newline \end{aligned} $$
Since we don't care how long the movement takes, let's choose arbitrarily that the movement should last \(1\) second:
$$ Duration = 1 $$
then we have the coefficients
$$ \begin{aligned} A &= (2 \cdot -(\pi/2)/1 + 0 + 0)/1^2 = -\pi \newline B &= (3 \cdot (\pi/2)/1 - 2 \cdot 0 -0)/1 = 3\pi/2 \newline C &= 0 \newline D &= 0 \newline \end{aligned} $$
Plugging these values back into the cubic equations, we can see in the figure that the joint at \(t = 1s\) has position \(Q = \pi/2>rad\) and velocity \(v = 0>rad/s\).
Figure: Joint Position, Velocity, and Acceleration over Time
Joint Constraints
In reality, a joint may not physically be able to move in 1 second, so let's consider some realistic constraints.
Say the joint has a maximum angular velocity of \(6°/s\) and a maximum angular acceleration of \(3°/s^2\). Assume this holds true regardless of its payload or position, i.e. ignore dynamics.
$$ \begin{aligned} V_{limit} &= 0.104719755 > rad/s \newline A_{limit} &= 0.0523599 > rad/s^2 \end{aligned} $$
Clearly the solution plotted above exceeds these limits:
$$ \begin{aligned} V_{max} &= 3\pi/4, t = 0.5s \newline A_{max} &= 3\pi, t = 0s \newline &= -3\pi, t = 1s \newline \end{aligned} $$
We can reduce the velocity and acceleration by scaling the duration, i.e. making the movement take longer. The time-optimal solution is found analytically according to Melchiorri [1]:
double get_scale()
{
double v_scale = abs(Vmax) / Vlimit
double a_scale = sqrt(abs(Amax) / Alimit)
return max(v_scale, a_scale)
}
If a_scale is larger than v_scale, then the acceleration limit is constraining the duration. If v_scale is larger than a_scale, then the velocity limit is constraining the duration.
v_scale = abs(3π/4) / 0.104719755 = 22.5
a_scale = sqrt(abs(3π)/0.0523599) = 13.417
In this case, the velocity limit is the dominating constraint. The time-optimal duration is 22.5 seconds.
We can verify by recalculating the polynomial coefficients with the new duration.
$$ \begin{aligned} A &= (2 \cdot -(\pi/2)/22.5)/22.5^2 = -0.00027580511 \newline B &= (3 \cdot (\pi/2)/22.5)/22.5 = 0.00930842267 \newline C &= 0 \newline D &= 0 \newline \end{aligned} $$
Figure: Scaled Joint Position, Velocity, and Acceleration over Time
We can see that the joint at \(t = 22.5s\) has position \(Q = \pi/2 >rad\) and velocity \(v = 0 >rad/s\). The maximum velocity is at \(t = 11.25s\) with \(v = 0.104719755 >rad/s\). The maximum acceleration is at \(t = 0\) and \(t = 22.5s\) with \(a = 0.0186 >rad/s^2\) and \(a = -0.0186 >rad/s^2\), respectively. The joint velocity and acceleration constraints are satisfied. \(\blacksquare\)
Task Space Constraints
Let's add another constraint. Let's say the frame attached to the tip of the joint has maximum translational speed and angular velocity components.
Figure: Diagram of a frame at the joint tip. The frame is right-handed, i.e. Z points out of the page.
$$ \begin{aligned} \dot{X}_{max} &= 100mm/s \newline \dot{Y}_{max} &= 100mm/s \newline \dot{Z}_{max} &= 100mm/s \newline \dot{R_x}_{max} &= 9°/s \newline \dot{R_y}_{max} &= 9°/s \newline \dot{R_z}_{max} &= 9°/s \newline \end{aligned} $$
Aside: I say components because e.g. a velocity vector moving with \(\dot{X} = \dot{Y} = \dot{Z} = 100mm/s\) would actually be moving at \(\sqrt{(100²+100²+100²)} ~= 173mm/s\). One could certainly solve for a velocity vector constraint, too.
Similar to joint space constraints, we can meet task space constraints by scaling the duration of the trajectory, but we need to know the relation from joint space to task space.
The relation from joint space to task space is known as forward kinematics. Conversion from joint position to task space position is forward position, and conversion from joint velocity to task space velocity is forward velocity. This topic is widely covered elsewhere.
Let's say our robot joint has position \(Q\), angular velocity \(\dot{Q}\) (also known as \(V(t)\)), and radius \(r\) from its center of rotation to the tip frame. Then the following relations apply:
$$ \begin{aligned} X &= r \cdot cos(Q) \newline Y &= r \cdot sin(Q) \newline Z &= 0 \newline R_x &= 0 \newline R_y &= 0 \newline R_z &= Q \newline \newline \dot{X} &= -\dot{Q} \cdot r \cdot sin(Q) \newline \dot{Y} &= \dot{Q} \cdot r \cdot cos(Q) \newline \dot{Z} &= 0 \newline \dot{Rx} &= 0 \newline \dot{Ry} &= 0 \newline \dot{Rz} &= \dot{Q} \newline \end{aligned} $$
For example, if \(r = 1 >meter\), \(Q = 0 >rad\), and \(Qdot = \pi > rad/s\), then
$$ \begin{aligned} X &= 1m \newline Y &= 0m \newline R_z &= 0m \newline \dot{X} &= 0 >m/s \newline \dot{Y} &= 1 >m/s \newline \dot{R_z} &= \pi > rad/s \newline \end{aligned} $$
For another example, if \(r = 1 >meter\), \(Q = \pi/2 >rad\), and \(Qdot = \pi > rad/s\), then
$$ \begin{aligned} X &= 0m \newline Y &= 1m \newline R_z &= 0m \newline \dot{X} &= -1 >m/s \newline \dot{Y} &= 0 >m/s \newline \dot{R_z} &= \pi > rad/s \newline \end{aligned} $$
For details, see this video lecture.
Going back to our 1-second trajectory, since the joint velocity is a parabola, which is symmetric, the maximum occurs at any of \(t = 0\), \(t = 0.5\), or \(t = 1\). Since \(V_{init} = V_{final} = 0\), the maximum occurs at \(t = 0.5\). This results in the following task space velocities:
$$ \begin{aligned} Q(0.5) &= -\pi t³ + \frac{3\pi}{2}t² = \pi/4 >rad \newline V(0.5) &= -3\pi(1/2)² +3\pi/2 = \frac{3\pi}{4} rad/s \newline \dot{X}_{max} &= \frac{3\pi}{4} rad/s \cdot 1m \cdot cos(\pi/4) = 1.67m/s = 1670 >mm/s \newline \dot{Y}_{max} &= \frac{3\pi}{4} rad/s \cdot 1m \cdot sin(\pi/4) = 1.67m/s = 1670 >mm/s \newline \dot{Z}_{max} &= 0 >mm/s \newline \dot{R_x} &= 0 >rad/s \newline \dot{R_y} &= 0 >rad/s \newline \dot{R_z} &= \frac{3\pi}{4} >rad/s \newline \end{aligned} $$
Dividing by the given task space constraints yields the following ratios:
$$ \begin{aligned} X_{ratio} &= 1670/100 = 16.7 \newline Y_{ratio} &= 1670/100 = 16.7 \newline Z_{ratio} &= 0 \newline R_{x_{ratio}} &= 0 \newline R_{y_{ratio}} &= 0 \newline R_{z_{ratio}} &= \frac{\frac{3\pi}{4} >rad/s}{9°/s} = 15 \newline \end{aligned} $$
The maximum task space ratio is \(16.7\), which is less than the previous value of \(v_{scale} = 22.5\). The previous scaled trajectory duration of 22.5s also satisfies the given task space constraints. \(\blacksquare\)
Multiple Joints
The same approach applies to robots with more than one joint.
In this case, if we have \(m\) joints, then we will have \(m\) cubic splines, and \(a_{scale}\) and \(v_{scale}\) must be calculated for each joint.
The scale resulting from dividing the forward velocity by the task space limit is also calculated. The maximum of the joint space ratios and the task space ratio yields the optimal trajectory duration.
Example
Consider adding a second joint to the previous example to create a two-joint manipulator. This joint has the same velocity and acceleration limits.
$$ \begin{aligned} V_{limit} &= 0.104719755 > rad/s \newline A_{limit} &= 0.0523599 > rad/s^2 \end{aligned} $$
Figure: A robot with two joints.
Simulate this robot on Desmos. Use the \(q_1\) and \(q_2\) sliders.
The first spline is unchanged.
$$ \begin{array}{c} \begin{aligned} Q_{init_1} &= 0 & A_1 &= -\pi \newline Q_{final_1} &= \pi/2 & B_1 &= 3\pi/2 \newline V_{init_1} &= 0 & C_1 &= 0 \newline V_{final_1} &= 0 & D_1 &= 0 \newline \end{aligned} \end{array} $$
Here is the second spline.
$$ \begin{array}{c} \begin{aligned} Q_{init_2} &= \pi/2 & A_2 &=2\pi \newline Q_{final_2} &= -\pi/2 & B_2 &= -3\pi \newline V_{init_2} &= 0 & C_2 &= 0 \newline V_{final_2} &= 0 & D_2 &= \pi/2 \newline \end{aligned} \end{array} $$
Here is the relation of joint space to task space.
$$ \begin{aligned} X &= r_1 \cdot cos(Q_1) + r_2 \cdot cos(Q_1 + Q_2) \newline Y &= r_1 \cdot sin(Q_1) + r_2 \cdot sin(Q_1 + Q_2) \newline Z &= 0 \newline R_x &= 0 \newline R_y &= 0 \newline R_z &= Q_1 + Q_2 \newline \newline \dot{X} &= -\dot{Q_1} \cdot r_1 \cdot sin(Q_1) - \dot{Q_1} \cdot \dot{Q_2} \cdot r_2 \cdot sin(Q_1 + Q_2) \newline \dot{Y} &= \dot{Q_1} \cdot r_1 \cdot cos(Q_1) + \dot{Q_1} \cdot \dot{Q_2} \cdot r_2 \cdot cos(Q_1 + Q_2)\newline \dot{Z} &= 0 \newline \dot{Rx} &= 0 \newline \dot{Ry} &= 0 \newline \dot{Rz} &= \dot{Q_1} + \dot{Q_2}\newline \end{aligned} $$
For details, see the derivation of position here and the derivation of velocity here.
We now find the maximum velocity, acceleration, and resulting time scale for each joint.
Note: The topic of finding polynomial minima or maxima is well-covered elsewhere and can be deferred to a good algebra library. Here, we just use Desmos.
Figure: Joint-space velocities, two-joint manipulator.
First Joint
$$ \begin{aligned} V_{1_{max}} &= 3\pi/4, t = 0.5s \newline A_{1_{max}} &= 3\pi, t = 0s \newline &= -3\pi, t = 1s \newline V_{1_{scale}} &= \frac{|3π/4|}{0.10471975} = 22.5 \newline A_{1_{scale}} &= \sqrt{\frac{|3π|}{0.0523599}} = 13.417 \newline \end{aligned} $$
Second Joint
$$ \begin{aligned} V_{2_{max}} &= 3\pi/2, t = 0.5s \newline A_{2_{max}} &= 6\pi, t = 0s \newline &= -6\pi, t = 1s \newline V_{2_{scale}} &= \frac{|3π/2|}{0.10471975} = 45 \newline A_{2_{scale}} &= \sqrt{\frac{|6π|}{0.0523599}} = 18.974 \newline \end{aligned} $$
Without considering task space velocity, the time-optimal duration is \(1s \cdot max(22.5, 13.417, 45, 18.974)=45s\). Clearly, since the second joint has twice as far to rotate as the first joint, it constrains the duration of the movement.
Let's now consider task space velocity.
Figure: Task-space velocities, two-joint manipulator.
$$ \begin{aligned} \dot{X}_{max} &= |3.245|m/s, t=0.3624s \newline \dot{Y}_{max} &= |-3.245|m/s, t=0.6376s \newline \dot{R_z}_{max} &= -3\pi/4 rad/s, t=0.5s \newline X_{ratio} &= 3245/100 = 32.45 \newline Y_{ratio} &= 3245/100 = 32.45 \newline R_{z_{ratio}} &= \frac{\frac{3\pi}{4} >rad/s}{9°/s} = 15 \newline \end{aligned} $$
Since \(32.45 < 45\), the joint space constraints dominate the task space constraints. The time-optimal duration is \(1s \cdot max(45, 32.45, 15)=45s\). \(\blacksquare\)
Longer Paths
If a path \(\pmb{Q}_j\) of length \(n | n>2\) is given for joint \(j\), i.e. intermediate points between \(Q_{init}\) and \(Q_{final}\) are given, and if the joint begins and ends with zero velocity (\(V_{init} = V_{final} = 0\)), then by enforcing the constraints of continuity on velocity and acceleration, the intermediate point velocities can be calculated with a system of linear equations following the method described by Melchiorri [1].
Example
Consider an extension of the previous 2-joint trajectory, where each joint passes through three positions: an initial position, an intermediate position, and a final position.
$$ \begin{aligned} \pmb{Q}_1 &= [Q_{1_{init}} Q_{1_{intermediate}} Q_{1_{final}}] \newline \pmb{Q}_2 &= [Q_{2_{init}} Q_{2_{intermediate}} Q_{2_{final}}] \newline \end{aligned} $$
Let the given intermediate positions be
$$ \begin{aligned} Q_{1_{intermediate}} &= \pi/4 \newline Q_{2_{intermediate}} &= 0 \end{aligned} $$
...along with the initial and final velocities.
\(V_{1_{init}} = V_{1_{final}} = V_{2_{init}} = V_{2_{final}}= 0\)
We must find the intermediate velocities \(V_{1_{intermediate}}\) and \(V_{2_{intermediate}}\). We can do this by solving the system
$$ A\pmb{v} = \pmb{c} $$
where
$$ A= \begin{bmatrix} 2(T_1+T_2) & T_1 \newline T_3 &2(T_2+T_3) &T_2 \newline & & \ddots \ddots \ddots \newline & & & T_{n-2} & 2(T_{n-3}+T_{n-2}) & T_{n-3} \newline & & & & T_{n-1} & 2(T_{n-2} + T_{n-1}) \newline \end{bmatrix} $$
$$ \pmb{v} = \begin{bmatrix} V_2 \newline V_3 \newline \vdots \newline V_{n-2} \newline V_{n-1} \newline \end{bmatrix} $$
$$ \pmb{c} = \begin{bmatrix} \frac{3}{T_1T_2}[T_1^2(Q_3-Q_2)+T_2^2(Q_2-Q_1)] \pmb{- T_2V_1} \newline \frac{3}{T_2T_3}[T_2^2(Q_4-Q_3)+T_3^2(Q_3-Q_2)] \newline \vdots \newline \frac{3}{T_{n-3}T_{n-2}}[T_{n-3}^2(Q_{n-1}-Q_{n-2})+T_{n-2}^2(Q_{n-2}-Q_{n-3})] \newline \frac{3}{T_{n-2}T_{n-1}}[T_{n-2}^2(Q_{n}-Q_{n-1})+T_{n-1}^2(Q_{n-1}-Q_{n-2})] \pmb{- T_{n-2}V_n}\newline \end{bmatrix} $$
and
$$ \begin{aligned} T_i &= Duration_i & \text{// The duration of segment \(i\)} \newline Q_i &= Q_{i_{init}} & \text{ // The initial position of segment \(i\) } \newline V_i &= V_{i_{init}} & \text{ // The initial velocity of segment \(i\) } \newline V_n &= V_{n_{final}} & \text{ // The final velocity of the last segment } \newline \end{aligned} $$
Reminder: There are \(n-1\) splines interpolating \(n\) control points (also called knots), and our indexing starts at \(1\), not \(0\). Therefore, \(i==1\) refers to the first spline, and \(i == n-1\) refers to the last spline.
Aside: Here is an example implementation of the above algorithm.
Joint 1
We solve for \(V_{1_{intermediate}}\).
$$ \begin{array}{ccc} \begin{bmatrix} 2(T_1+T_2) \end{bmatrix} \begin{bmatrix} V_{1_{intermediate}} \end{bmatrix} &= \begin{bmatrix} \frac{3}{T_1T_2}[T_1^2(Q_{1_{final}}-Q_{1_{intermediate}})+T_2^2(Q_{1_{intermediate}}-Q_{1_{init}})]-T_2 \cdot V_{1_{init}} \end{bmatrix} \end{array} $$
If we choose again arbitrarily that each segment should have 1 second of duration, then \(T_1=1\) and \(T_2=1\). Then
$$ \begin{aligned} \begin{bmatrix} 2(1+1) \end{bmatrix} \begin{bmatrix} V_{1_{intermediate}} \end{bmatrix} &= \begin{bmatrix} \frac{3}{1 \cdot 1}[1^2(\pi/2-\pi/4)+1^2(\pi/4-0)]-2\cdot 0 \end{bmatrix} \newline 4 \cdot V_{1_{intermediate}}&= \begin{bmatrix} 3[\pi/4+\pi/4] \end{bmatrix} \newline V_{1_{intermediate}} &= 3\pi/8 \ rad/s \end{aligned} $$
We can now plot the two splines.
$$ \begin{aligned} Duration_{1 \rightarrow 2} &= 1 \newline Displacement_{1 \rightarrow 2} &= \pi/4 \newline A_{1 \rightarrow 2} &= \frac{(2 \cdot - \pi/4 + 0 + 3\pi/8)}{1^2} &&= -\pi/8\newline B_{1 \rightarrow 2} &= \frac{(3 \cdot \pi/4 - 2 \cdot 0 - 3\pi/8)}{1} &&= 3\pi/8\newline C_{1 \rightarrow 2} &= V_{init} &&= 0\newline D_{1 \rightarrow 2} &= Q_{init} &&= 0\newline \end{aligned} $$
$$ \begin{aligned} Duration_{2 \rightarrow 3} &= 1 \newline Displacement_{2 \rightarrow 3} &= \pi/4 \newline A_{2 \rightarrow 3} &= \frac{(2 \cdot - \pi/4 + 3\pi/8 + 0)}{1^2} &&= -\pi/8\newline B_{2 \rightarrow 3} &= \frac{(3 \cdot \pi/4 - 2 \cdot 3\pi/8 - 0)}{1} &&= 0\newline C_{2 \rightarrow 3} &= V_{init} &&= 3\pi/8\newline D_{2 \rightarrow 3} &= Q_{init} &&= \pi/4\newline \end{aligned} $$
Figure: Two cubic splines for joint 1. Can you see where they meet? Hint: Each spline has 1s of duration.
We only see one spline, but there are actually two. The first spline is valid on the interval \(t=(0,1)\), and the second spline is valid on the interval \(t=(1,2)\). The two splines are identical because they have the same duration, displacement, and absolute change in velocity. Therefore, while we chose to use two splines, this movement could have been interpolated by a single spline.
Joint 2
We solve for \(V_{2_{intermediate}}\).
$$ \begin{aligned} \begin{bmatrix} 2(1+1) \end{bmatrix} \begin{bmatrix} V_{2_{intermediate}} \end{bmatrix} &= \begin{bmatrix} \frac{3}{1 \cdot 1}[1^2(-\pi/2-0)+1^2(0-\pi/2)]-2\cdot 0 \end{bmatrix} \newline 4 \cdot V_{2_{intermediate}}&= \begin{bmatrix} 3[-\pi/2-\pi/2] \end{bmatrix} \newline V_{2_{intermediate}} &= -3\pi/4 \ rad/s \end{aligned} $$
$$ A = \frac{(2 \cdot -Displacement / Duration + V_{init} + V_{final})}{Duration^2} \newline B = \frac{(3 \cdot Displacement / Duration - 2 \cdot V_{init} - V_{final})}{Duration} \newline $$
$$ \begin{aligned} Duration_{1 \rightarrow 2} &= 1 \newline Displacement_{1 \rightarrow 2} &= -\pi/2 \newline A_{1 \rightarrow 2} &= \frac{(2 \cdot - (- \pi/2) + 0 + (- 3\pi/4))}{1^2} &&= \pi/4\newline B_{1 \rightarrow 2} &= \frac{(3 \cdot (- \pi/2) - 2 \cdot 0 - (-3\pi/4))}{1} &&= -3\pi/4\newline C_{1 \rightarrow 2} &= V_{init} &&= 0\newline D_{1 \rightarrow 2} &= Q_{init} &&= \pi/2\newline \end{aligned} $$
$$ \begin{aligned} Duration_{2 \rightarrow 3} &= 1 \newline Displacement_{2 \rightarrow 3} &= -\pi/2 \newline A_{2 \rightarrow 3} &= \frac{(2 \cdot - (-\pi/2) + (- 3\pi/4) + 0)}{1^2} &&= \pi/4\newline B_{2 \rightarrow 3} &= \frac{(3 \cdot (- \pi/2) - 2 \cdot (-3\pi/4) - 0)}{1} &&= 0\newline C_{2 \rightarrow 3} &= V_{init} &&= -3\pi/4\newline D_{2 \rightarrow 3} &= Q_{init} &&= 0\newline \end{aligned} $$
Figure: Two cubic splines for joint 2. They meet at \(t=1s\).
Scaling Longer Trajectories
In order to satisfy joint-space and task-space velocity and acceleration limits, the \(m(n-1)\) resulting cubic splines (\(m=\) number of joints, \(n\) = number of control points) must each be scaled by the method described previously.
Pseudocode:
// segments: sequence of splines, lenth n - 1
// (n == number of interpolated points)
// task_space_limit: e.g. Xlim = 100, Ylim = 100mm/s, etc.
void scale(segments, task_space_limit):
for segment in segments:
joints = segment.joints // joints: container of size m
// get_scale: See Joint Constraints section
joint_space_ratio = joints.max(joint => joint.get_scale())
task_space_ratio = joints.forward_velocity() / task_space_limit
r_max = max(joint_space_ratio, task_space_ratio)
scale(segment, r_max)
Above: For each segment \(i\), \(m\) ratios are calculated, one for each joint, and the task space ratio is calculated. The maximum ratio \(r_{max}\) is selected. Then all \(m\) splines at segment \(i\) are scaled by \(r_{max}\).
Pseudocode:
// Scale the given spline
void scale(segment, ratio):
segment.duration *= ratio // Update duration
// Update velocity
if (segment.is_first()) spline.Vinit /= ratio
if (segment.is_last()) spline.Vfinal /= ratio
if (segment.is_intermediate()) spline.Vintermediate /= ratio
forward_propagate(segment.next);
segment.compute_velocities() // e.g. find new Vintermediate
segment.compute_coefficients() // e.g. get A B C D
// Update segment start and finish times
void forward_propagate(segment):
while (next_segment != null):
next_segment.forward_propagate() // e.g. Tinit = Tinit_prev + Duration
next_segment= segment.next
Above: When a segment is scaled, the following changes occur
- the segment duration
- the segment initial or final velocity (or both)
Forward propagation: All segments after the scaled segment are affected: the start time of each spline becomes the finishing time of the previous. These values propagate to the last spline.
Finally, the resulting intermediate velocities and polynomial coefficients must be recomputed.
References
[1] Biagiotti, L., & Melchiorri, C. (2009). Trajectory Planning for Automatic Machines and Robots. Berlin, Heidelberg: Springer Berlin Heidelberg.