A new method of optimal cost trajectory planning based on a graph searching algorithm is presented. Various heuristic functions which play the key role in the construction of effective algorithms for solving such problems are proposed. Some numerical examples are given based on the kinematic and dynamic models of a IRb-6 ASEA robot. This method rooted in AI has less computational complexity than a dynamical programming method applying to the same optimal-cost trajectory planning problem.