function p = exampleHelperPolynomialTrajectory(q0, q1, tf)
    %exampleHelperPolynomialTrajectory generate vector to vector trajectory using 6-order polynomial
    %
    %   The trajectory starts at time = 0, and ends at time = tf. It is a
    %   simple example of trajectory generation that assumes zero velocity
    %   and acceleration at the beginning and the end of the trajectory.
    %
    %   Copyright 2017 The MathWorks, Inc.
    
    % Reshape q0 and q1 as row vectors
    q0 = reshape(q0, 1, []);
    q1 = reshape(q1, 1, []);
    
    % Create zero velocity and acceleration vectors for end points
    dim = size(q0,2);
    dq0 = zeros(1,dim);
    dq1 = zeros(1,dim);
    ddq0 = zeros(1,dim);
    ddq1 = zeros(1,dim);
    
    Ainv = [...
        [  -6/tf^5, -3/tf^4, -1/tf^3,   6/tf^5, -3/tf^4, 1/(2*tf^3)];...
        [  15/tf^4,  8/tf^3,  3/tf^2, -15/tf^4,  7/tf^3,    -1/tf^2];...
        [ -10/tf^3, -6/tf^2,   -3/tf,  10/tf^3, -4/tf^2,   1/(2*tf)];...
        [       0,      0,      1,       0,      0,         0];...
        [       0,      1,      0,       0,      0,         0];...
        [       1,      0,      0,       0,      0,         0]    ];
    
    p = zeros(6,dim);
    for i = 1: dim
        p(:,i) = Ainv* [q0(i); dq0(i); ddq0(i); q1(i); dq1(i); ddq1(i)];
    end
end