Completed implementation of inverse kinematics