手里有个双连杆的三轴教育用途的机械臂，带有一个末端工具吸盘，其长度和位置都已知。其末端还有一段结构，能保证吸盘姿态始终保持90°垂直向下。

厂家提供的运动学库越靠近工作空间边缘精度差的越多，位置精度无法保证。

打算自己写运动学库，又发现，无法让末端工具始终保持垂直90°。针对这种末端工具，也不知道逆运动学改怎么写。

自己写的正运动学和DH参数，如下：

运行的仿真效果如下，最后一截代表吸盘的那一段无法保证始终垂直朝下：

机械臂实物图：

有人玩过类似的机械臂吗？可以提供点经验吗？