function [Q,dQ,ddQ,counter] = setJointPosition( t,tf,qi,qf,pdeg )
%__________________________________________________________________________
% in_args: t, time instant
%          tf, execution time
%          qi, actual (initial) joint vector
%          qf, final joint vector
%          pdeg, order of the polynomial function @{3,5}
% out_args:   Q, position time course
%            dQ, velocity time course
%           ddQ, acceleration time course
%           counter, number of joints with null-motion
 DIM = length(t);
% Polynomial trajectories
 counter = 0;
 for nn = 1:length(qf), % Trajectory planning for each joint
     if abs(qf(nn)-qi(nn)) > 1e-10,
        [Q(nn,:),dQ(nn,:),ddQ(nn,:)]=PolyTrajGenerator(t,tf,qi(nn),qf(nn),pdeg);
     else
         counter = counter+1;
           Q(nn,:) = qi(nn)*ones(1,DIM);
          dQ(nn,:) = zeros(1,DIM);
         ddQ(nn,:) = zeros(1,DIM);
     end 
 end
%__________________________________________________________________________
end

