Pybulle中确定UR机械臂末端姿态方法

在Pybullet仿真环境中确定机械臂末端姿态总是一件令人头痛的事情，什么RPY、欧拉角绕哪个坐标轴旋转，等等一些绕来绕去的非常混乱，依然不明白期望的末端姿态应该如何设置。因此，本文详细梳理了如何通过旋转XYZ欧拉角得到我们期望的末端姿态。

主要使用的函数为getQuaternionFromEuler，阅读pybullet_quickstartguide手册，可以了解：
The pybullet API uses quaternions to represent orientations. Since quaternions are not very intuitive for people, there are two APIs to convert between quaternions and Euler angles.
The getQuaternionFromEuler input arguments are:
getQuaternionFromEuler returns a list of 3 floating point values, a vec3.

大致意思为使用四元数表示姿态具有很多优点（避免奇异点），但是四元数对于人们不是很直观，因此Pybullet内置的API可以将欧拉角转换为四元数。函数getQuaternionFromEuler输入的是绕XYZ轴旋转的角度，顺序不要错了，分别绕XYZ轴旋转。这里的XYZ是固定的世界坐标系。
具体步骤：
1、首先确定机械臂末端坐标系：
a.通常情况下机械臂末端接近物体的方向为Z轴，即接近矢量a（approach）
b.若夹爪为两指夹爪，两指的连线方向为Y轴，即方位矢量o（orientation）
c.通过右手法则确定X轴，即法向矢量n（normal）
reference：华科大赵欢老师机器人学导论课程

2、其次确定机械臂末端绕XYZ轴转0度时的初始姿态，即getQuaternionFromEuler（0，0，0）
3、机械臂初始姿态下，末端坐标系如何通过绕世界坐标系的XYZ轴转动到达我们期望的末端姿态，这里是分别绕XYZ轴转动，顺序千万不要弄错。例如，末端姿态朝下（Z轴朝下），便于桌面级抓取；又或是末端姿态朝前（Z轴朝前），便于水果采摘等等。

注意事项：
A、绕世界坐标XYZ轴的旋转，始终是绕固定的世界坐标系。
B、仔细考虑旋转角度的正负号，世界坐标系的箭头朝向自己，逆时针为正，顺时针为负。

例子：
此时为机械臂在getQuaternionFromEuler（0，0，0）时的初始姿态。

若希望末端姿态朝下抓取，只需绕世界坐标系的Y轴旋转pai/2的角度，即
getQuaternionFromEuler（0，pai/2，0）
当然也有其他方式，例如：
getQuaternionFromEuler（-pai，pai/2，0）

逻辑正确，完结撒花！！！