Fork from http://wiki.ros.org/interpolated_ik_motion_planner