Interpolating pose in 3D

Finally, we come to the problem of interpolating between two poses. Poses have got a translational and a rotational component. We need to interpolate them separately. We refer to this as Cartesian interpolation. Let’s consider two poses; pose 0 and pose 1, each can be represented by a rotation matrix and a vector. We can convert the rotation matrix of each pose into a unique quaternion and we can interpolate the translation using the linear interpolation formula that we introduced earlier. S is a function of time, varies smoothly from 0 to 1 over the duration of the trajectory. And, we interpolate the rotation using the quaternion interpolation formula that we introduced in the last section. If we want, we can convert the interpolated quaternion back to a rotation matrix, combine it with the interpolated translation and rebuild a homogeneous transformation matrix.

Let’s now interpolate between two poses described in terms of homogeneous transformation matrices. The first pose is a null pose. It just got 0 orientation and a translational component of 0. The second pose has a finite translational component and a finite rotational component and is defined by this homogeneous transformation matrix here. Now, I can create a smooth trajectory between these two poses using the Toolbox function, ctraj which stands for Cartesian trajectory. And, the arguments are the initial pose, the final pose and the number of time steps. The result is a matrix that’s 4 x 4 x 50. Once again, this is a stack of homogeneous transformation matrices. The first element in the stack, look at all the rows and all the columns and the first in the stack looks like this. But, the 10th one in the stack, look at all the rows and all the columns and the 10th in the stack looks like this.

Each homogeneous transformation in this stack represents a pose along a trajectory and pose varies smoothly along this trajectory, smoothly in terms of translation and smoothly in terms of orientation. I can plot this using the tranimate function and we see here that the pose is changing from the initial pose to the final pose smoothly in position and in orientation.