Inverse Kinematics, not sure if it works

jacobian seems to be screwing up as well