Working version of planner with manipulator as of Mar 15, 2013.