Two stage inverse kinematics algorithm, first inner joint variables, then outer