First commit: working inverse kinematics code