Ported IK function to python, wrote quick FK python code, and added some capability for plotting working envelope