%% Interpolate Between Homogenous Transformations
% Build transformations from two orientations and positions. Specifiy the time interval and vector of times for interpolating.

t0 = axang2tform([0 1 1 pi/4])*trvec2tform([0 0 0]);
tF = axang2tform([1 0 1 6*pi/5])*trvec2tform([1 1 1]);
tInterval = [0 1];
tvec = 0:0.01:1;

%
% Interpolate between the points. Plot the trajectory using plotTransforms. Convert the transformations to quaternion rotations and linear transitions. The figure shows all the intermediate transformations of the coordinate frame.

[tfInterp, v1, a1] = transformtraj(t0,tF,tInterval,tvec);

rotations = tform2quat(tfInterp);
translations = tform2trvec(tfInterp);

plotTransforms(translations,rotations)
xlabel('X')
ylabel('Y')
zlabel('Z')

% Copyright 2018 The MathWorks, Inc.