PUMA560机器人运动学反解算法，摘自熊有伦《机器人基础》一书。