企业官网建设流程全解析

7、UR5e机械臂的速度运动学代码

1）从XML文件里读取每个关节的角度和角速度，即<key qpos="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0" ctrl="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0"/>

2） MuJoCo正向运动学计算每个连杆坐标系原点、每个连杆质心、末端相对于基座的速度

3）关节的角度通过自定义的正向运动学得到连杆之间的变换矩阵，包含旋转R和平移

关节的角速度以及连杆之间的变换矩阵通过自定义的速度运动学得到每个连杆坐标系原点、每个连杆质心、末端相对于基座的速度

这里关节角度q，关节的角速度u

4） 打印计算结果对比验证

importnumpyasnpimportutilityasramfromtypesimportSimpleNamespacefromforward_