Efficient Inverse Kinematics for General 6R Manipulators
Robot Dynamics And Control