unity中的机器人逆运动学

现在我们使用网上下载的基于右手坐标系的puma560的逆运动学代码实现了xyz的平动，但是还是无法实现旋转，现在我们用matlab里的算出来的角度数据手动输入到unity里发现它可以实现目标位姿，可能是我们的代码中的计算出了问题，现在求matlab中计算puma560机器人的代码转换为Csharp后的形式。

可以尝试使用插件
BioIK
FinalIK
这两个我都比较熟悉