  Motion planning for multi-jointed robots is challenging. Due to the inherent
complexity of the problem, most existing works decompose motion planning as
easier subproblems. However, because of the inconsistent performance metrics,
only sub-optimal solution can be found by decomposition based approaches. This
paper presents an optimal control based approach to address the path planning
and trajectory planning subproblems simultaneously. Unlike similar works which
either ignore robot dynamics or require long computation time, an efficient
numerical method for trajectory optimization is presented in this paper for
motion planning involving complicated robot dynamics. The efficiency and
effectiveness of the proposed approach is shown by numerical results.
Experimental results are used to show the feasibility of the presented planning
algorithm.
