Moving 6dof Jacobian planner into this repo