The servo constraint following control algorithm based on Udwadia and Kalaba equation requires the consistent initial conditions. To solve this problem, the principle of generalized virtual displacement decomposition is used to transform constraint errors into generalized coordinate errors and generalized velocity errors. The concept of total error of trajectory tracking for mechanical systems is proposed, and the explicit control forces formulae are deduced to eliminate this error. The method proposed here can compensate the initial tracking errors. Because of the controllability of this tracking control algorithm on initial errors, it can be combined with servo constraint following control which is based on the Udwadia and Kalaba equation. This method can handle the trajectory tracking control under inconsistent initial conditions effectively. The numerical simulation of a mechanical manipulator with two degrees of freedom is made and the results show that the algorithm can solve problems caused by the inconsistent initial conditions, also solve constraint violation problem, and improve the control precision and stability.