function [Pee,dPee,ddPee] = CartTrajPlanning( t,tf,qi,XYZ_f,L,warm,pdeg )
%__________________________________________________________________________
% in_args: t, time vector
%          F, function to estimate the derivative
% out_args: dF, first derivative
%          ddF, second derivative
%
 DIM = length(t);
% Get current hip XYZ_coordinates  
 P = DirKin_4dof(qi,L,warm);
 XYZ_i = P(:,3);
% Polynomial trajectories
   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

