semi-working IK
changed FK solving method