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 from to degrees.
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.
We can satisfy these constraints by interpolating the joint position, velocity, and acceleration with a cubic spline,
where is the time since the movement started.
We start by finding the coefficients , , , and .
Since we don’t care how long the movement takes, let’s choose arbitrarily that the movement should last second:
then we have the coefficients
Plugging these values back into the cubic equations, we can see in the figure that the joint at has position and velocity .
Figure: Joint Position, Velocity, and Acceleration over Time
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 and a maximum angular acceleration of . Assume this holds true regardless of its payload or position, i.e. ignore dynamics.
Clearly the solution plotted above exceeds these limits:
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 :
double v_scale = abs(Vmax) / Vlimit
double a_scale = sqrt(abs(Amax) / Alimit)
return max(v_scale, a_scale)
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.
Figure: Scaled Joint Position, Velocity, and Acceleration over Time
We can see that the joint at has position and velocity . The maximum velocity is at with . The maximum acceleration is at and with and , respectively. The joint velocity and acceleration constraints are satisfied.
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.
Aside: I say components because e.g. a velocity vector moving with would actually be moving at . 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 , angular velocity (also known as ), and radius from its center of rotation to the tip frame. Then the following relations apply:
For example, if , , and , then
For another example, if , , and , then
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 , , or . Since , the maximum occurs at . This results in the following task space velocities:
Dividing by the given task space constraints yields the following ratios:
The maximum task space ratio is , which is less than the previous value of . The previous scaled trajectory duration of 22.5s also satisfies the given task space constraints.
The same approach applies to robots with more than one joint.
In this case, if we have joints, then we will have cubic splines, and and 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.
Consider adding a second joint to the previous example to create a two-joint manipulator. This joint has the same velocity and acceleration limits.
Figure: A robot with two joints.
Simulate this robot on Desmos. Use the and sliders.
The first spline is unchanged.
Here is the second spline.
Here is the relation of joint space to task space.
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.
Without considering task space velocity, the time-optimal duration is . 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.
Since , the joint space constraints dominate the task space constraints. The time-optimal duration is .
If a path of length is given for joint , i.e. intermediate points between and are given, and if the joint begins and ends with zero velocity (), 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 .
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.
Let the given intermediate positions be
…along with the initial and final velocities.
We must find the intermediate velocities and . We can do this by solving the system
Reminder: There are splines interpolating control points (also called knots), and our indexing starts at , not . Therefore, refers to the first spline, and refers to the last spline.
Aside: Here is an example implementation of the above algorithm.
We solve for .
If we choose again arbitrarily that each segment should have 1 second of duration, then and . Then
We can now plot the two splines.
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 , and the second spline is valid on the interval . 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.
We solve for .
Figure: Two cubic splines for joint 2. They meet at .
Scaling Longer Trajectories
In order to satisfy joint-space and task-space velocity and acceleration limits, the resulting cubic splines ( number of joints, = number of control points) must each be scaled by the method described previously.
void scale(segments, task_space_limit):
for segment in segments:
joints = segment.joints
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)
Above: For each segment , ratios are calculated, one for each joint, and the task space ratio is calculated. The maximum ratio is selected. Then all splines at segment are scaled by .
void scale(segment, ratio):
segment.duration *= ratio
if (segment.is_first()) spline.Vinit /= ratio
if (segment.is_last()) spline.Vfinal /= ratio
if (segment.is_intermediate()) spline.Vintermediate /= ratio
while (next_segment != null):
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.
 Biagiotti, L., & Melchiorri, C. (2009). Trajectory Planning for Automatic Machines and Robots. Berlin, Heidelberg: Springer Berlin Heidelberg.
Please feel free to start a discussion on GitHub.