Pick-up function written and visualization works. Still need to fix the the way a final state is chosen and if there are are any problems with the final angle being making the joint rotate 360 degrees.