Removed all the numpy.unwrap stuff. That was stupid.
The entire run-through seems to be working. Changed a few constants, removed the stupid numpy.unwrap stuff, removed unnecessary code, included rospy.loginfo statements at each step, made regrasping more robust by checking multiple values of angles along the needle to regrasp, also changed a few actions here and there for overall better running.