Interpolating rotation in 3D

We talked about how we can linearly interpolate position. What do we do for the case of rotations? We could naively rewrite our expression like this where we have the initial rotation, the final rotation and the intermediate rotation. But, there’s a problem with this scheme. A rotation matrix is not just any old 3 x 3 matrix. It’s a special matrix. It’s an orthogonal matrix and that means that each column in the matrix is a unit vector. So if I add two orthogonal matrices, the result will never be an orthogonal matrix because if I can see that each column, the sum of two unit vectors, can never ever be a unit vector. So this is not a strategy we can use to interpolate rotations.

We need to consider other ways of representing rotation that we can interpolate. So, one way to achieve this is to recall that this interpolation formula works for vectors and we can represent a rotation in terms of a vector of angles. It can be three Euler angles or it could be three roll, pitch, yaw angles. So if we consider this vector of angles as the symbol Gamma, then we can introduce the initial angles, the final angles, and then we can interpolate the intermediate angles. If our initial and final orientations are given in terms of rotation matrices, then we convert those two into initial set of angles and a final set of angles. We interpolate the intermediate angles and we can convert those back to a rotation matrix if we so desire. We’re going to use the function jtraj that we looked at a moment ago to interpolate roll, pitch, yaw angles. I’m going to put the result into the workspace variable X. Invoke the jtraj function. I’m going to interpolate between a set of roll, pitch, yaw angles and roll equal to 0 and the final set of roll, pitch, yaw angles which are equal to minus pi on 2, plus pi on 2 and pi on 4.

And, I’m going to do this interpolation over a hundred time steps. The result is a new variable in our workspace that’s got 100 rows, each row corresponds to a time step. And, it has got three columns and the columns correspond to the roll, pitch and yaw angles. I can plot this roll, pitch, yaw angles versus time. Here we can see what they look like, moving smoothly from the initial value to the final value. Now, we want to convert the roll, pitch, yaw angle in each row of the matrix X in to a rotation matrix. And to do that, I’m going to use the rpy2r function that we looked at previously and I’m going to pass in X. And what the function will do in this case is it will convert every row of this matrix into a rotation matrix. The result is a rotation matrix, but the matrix has got dimensions of 3 x 3 x 100. So we can think of it as a stack of rotation matrices.

So we could look at the first one in that stack and it’s a rotation matrix. It looks like this. It’s null rotation. It’s a unit matrix. I could look at the 10th one in the stack and it looks something like this. So R is a sequence of rotation matrices. Now I can pass a sequence of rotation matrices like that to the function tranimate. What we will see is an animation showing the orientation of the coordinate frame changing smoothly from the initial orientation specified in terms of roll, pitch, yaw angles to a final orientation also specified in terms of roll, pitch, yaw angles.

Euler angles and roll, pitch, yaw angles are represented as a vector of three angles. Each element of these vectors we can consider as a point on a circle and the path from the initial angle to the final angle is an arc on the unit circle. Consider now the case where we have this initial angle and this final angle. And a naive interpolation would move us from the initial angle to the final angle the long way around the circle. What we need to remember with angles is that there are always two ways to get from A to B. There is potentially the long way and there is potentially the short way. So when we are interpolating each element in our angle sequence vector, it’s important that we choose the shortest path.

If we look at the angle as a function of time, the long path is given here in blue. We see we’re moving from 2 radians to -2 radians. Whereas the short path which is shown in red moves through an angle of pi radians, pops back at minus pi radians and then moves quickly to the destination point B of a value of -2 radians. Earlier, we introduce quaternions and this is the equation we used to interpolate between two quaternions. Once again, it’s a function of S and S varies from 0 to 1. And the important element in this equation is Theta and Theta is derived from the elements of the initial and final quaternion. This is referred to as spherical linear interpolation and it is the shortest and most direct path between any two orientations. And it corresponds to a constant angular velocity about a fixed axis in space.

Let’s create a pair of quaternions that we will interpolate between. The first one corresponds to a null rotation. We see that it’s got a vector component of 0. The second quaternion is a representation of a rotation about the x-axis by pi on 2. So, we’re going to pass in a rotation matrix. The quaternion class is going to turn that into a quaternion. Now, there are two quaternions in our workspace, one corresponding to no rotation, one corresponding to rotation of 90 degrees about the x-axis. To interpolate quaternions, we use the interp method. We apply that to the initial quaternion. And, the first argument is the final quaternion and the second argument is the position along a trajectory. So 0 is the first point on the trajectory. That’s the initial condition, the initial quaternion which is the null rotation. I can repeat that and ask for the final point in the interpolation and that is equal to the second quaternion. I can ask now for a point that’s midway between 0.5 and this is the interpolated quaternion. And of course, I can turn that back in to a rotation matrix using the R method.

Now, we might like to compare the effect of the different types of rotational interpolation and that’s a little bit difficult. It’s hard to visualize a particular rotation. But, one way we can do it is to consider the effect of a rotation on a particular vector. So here, I have chosen the x-axis which I can represent by a vector 1, 0, 0. And I can apply these three different rotation sequences to that vector. I can interpolate the initial and final Euler angles, the initial and final roll, pitch, yaw angles or the initial and final quaternions and they are shown by the different colored parts plotted here on the sphere. The quaternion path shown in white is the shortest and most direct path. We can see that the roll, pitch, yaw interpolation path has deviated significantly from it. It has taken a more indirect and longer path to get from the initial orientation to the final orientation. Quaternion interpolation is the gold standard.