A method for minimizing tracking errors in dynamic systems including obtaining desired trajectory data of the dynamic system, obtaining a set of constraints on at least one of the desired and actual trajectories of the dynamic system, obtaining a set of uniform or non-uniform rational B-splines having known original B-spline basis functions but unknown B-spline coefficients, applying a trajectory optimization process to the desired trajectory data including applying forward filtering to B-spline basis functions and utilizing the original and filtered B-spline basis functions to select optimal coefficients of the B-splines, and outputting an optimal motion command signal in response to the trajectory optimization process to the dynamic system such that a resultant actual trajectory is substantially equal to the desired trajectory while satisfying the set of constraints on the at least one of the desired and actual trajectories.