function [Pee,dPee,ddPee] = CartTrajPlanning( t,tf,XYZ_i,XYZ_f,L,pdeg )
%__________________________________________________________________________
% in_args: t, time instant
%          tf, execution time
%          qi, actual (initial) joint vector
%          XYZ_f, final tooltip coordinates 
%          L, arm's link lengths
%          warm, which arm: left +1 or right -1
%          pdeg, order of the polynomial function @{3,5}
% out_args:   Pee, position time course
%            dPee, velocity time course
%           ddPee, acceleration time course
%
 DIM = length(t);
% Polynomial trajectory profiles
   Pee = zeros(3,DIM); % Pre-allocating for speed
  dPee = zeros(3,DIM); % Pre-allocating for speed
 ddPee = zeros(3,DIM); % Pre-allocating for speed
 for nn = 1:3, % Trajectory planning for each direction X,Y,Z
     if abs(XYZ_f(nn)-XYZ_i(nn)) > 1e-10,
        [Pee(nn,:),dPee(nn,:),ddPee(nn,:)] = ...
        PolyTrajGenerator(t,tf,XYZ_i(nn),XYZ_f(nn),pdeg);
      else
            Pee(nn,:) = XYZ_i(nn)*ones(1,DIM);
           dPee(nn,:) = zeros(1,DIM);
          ddPee(nn,:) = zeros(1,DIM);
      end
 end
%__________________________________________________________________________
end

