IK modified to use transformation matrices