function [ P,dP,ddP,counter ] = PolyTrajGenerator( t,Pi,Pf,pdeg )
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
%__________________________________________________________________________
% in_args: t, time instant
%          tf, execution time
%          Pi, actual (initial) vector
%          Pf, final vector
%          pdeg, order of the polynomial function @{3,5}
% out_args:   P, position time course
%            dP, velocity time course
%           ddP, acceleration time course
%           counter, number of joints with null-motion
 DIM = length(t);
 tf = t(end);
% Polynomial trajectories
 counter = 0;
 if abs(Pf-Pi) > 1e-10,
     switch pdeg,
         case 3, % 3rd-order polynomial: ZERO initial and final velocities
             P = -2*(Pf-Pi)*t.^3/tf^3+3*(Pf-Pi)*t.^2/tf^2+Pi;
             dP = -6*(Pf-Pi)*t.^2/tf^3+6*(Pf-Pi)*t/tf^2;
             ddP = -12*(Pf-Pi)*t/tf^3+6*(Pf-Pi)/tf^2;
         case 5, % 5th-order polynomial: ZERO initial/final vel & acceleration
             P = 6*(Pf-Pi)*t.^5/tf^5-15*(Pf-Pi)*t.^4/tf^4+10*(Pf-Pi)*t.^3/tf^3+Pi;
             dP = 30*(Pf-Pi)/tf^5*t.^4-60*(Pf-Pi)/tf^4*t.^3+30*(Pf-Pi)/tf^3*t.^2;
             ddP = 120*(Pf-Pi)/tf^5*t.^3-180*(Pf-Pi)/tf^4*t.^2+60*(Pf-Pi)/tf^3*t;
         otherwise, 
               disp('Unknown order!!!')
               P = NaN, dP = NaN; ddP = NaN;
     end
 else
           counter = 1;
           P = Pi*ones(1,DIM);
          dP = zeros(1,DIM);
         ddP = zeros(1,DIM);
end 
%__________________________________________________________________________
end