Working on IKFast and modifying raven_planner