pybullet中欧拉角姿态可视化及顺序记录（画机械臂末端坐标）

最近除了玩ros 也开始了pybullet的学习和使用，毕竟在不花钱的情况下做DRL，pybullet确实提供了太多的便利。

安装和基本使用略过，非常简单，直接pip install pybullet 就可以，无非会检查当前机器的c++编译器的版本，细节不说了。

主要记录下在pybullet中坐标变换，以及欧拉角的约定（这里说的就是 pybullet，有扛精说bullet里面本来如何如何的，请直接绕道）。

先上例子，我本来是要试一下用panda机械臂控制夹取，但是在pybullet中的末端法兰的坐标定义以及位置获取一直没有搞清楚以至于我后续工作没法清晰的开展。

首先，获取夹爪末端的空间位置可以通过p.getLinkState(id,11)获取(其中11为panda机械臂的末端link的索引)，返回的东西还比较多，官方api文档介绍如下：

而我们关心的末端位置和姿态对应返回的第0，1 个参数，而其坐标系位置与坐标系姿态对应返回第4，5个参数（索引从0开始），我通过对末端坐标系添加辅助线（p.addUserDebugLine）的方式摸清坐标的定义以及欧拉角的计算方式。

具体如下，我在模拟过程中首先通过逆运动把末端的初始姿态确定下来。

# panda 机械臂中可操作的 joint 共9个，索引如下，可通过 p.getJointInfo 获取：
joint_active_ids = [0,1,2,3,4,5,6,9,10] 

# 初始关节位置（随便找的位置）
rest_poses = [0,-0.215,0,-2.57,0,2.356,2.356,0.08,0.08]

# 把机械臂初始姿态摆出来
p.setJointMotorControlArray(panda_arm_ID,joint_active_ids, p.POSITION_CONTROL, targetPositions=rest_poses)


# 获取初始姿态下末端的信息（末端的位置和末端的姿态）
ini_p,ini_o = p.getLinkState(panda_arm_ID,11)[:2]


# 逆运动要到达的姿态 对应的欧拉角描述； 
euler_angs = [0,0,0] # 全部给0， 意味着要让机械臂末端坐标与世界坐标重合

# euler_angs = [0, np.pi/2, 0]
# euler_angs = [0, np.pi/2, np.pi/2]

# 转为四元数
robotEndOrientation = p.getQuaternionFromEuler(euler_angs) 

# 逆运动求解关节位置
joint_states = p.calculateInverseKinematics(panda_arm_ID, 11, ini_p,
        targetOrientation=robotEndOrientation)

# 设定关节位置
p.setJointMotorControlArray(panda_arm_ID,joint_active_ids,
            p.POSITION_CONTROL,targetPositions=joint_states)



# 末端坐标系的轴向：

# x 轴
frame_start_postition, frame_posture = p.getLinkState(panda_arm_ID,11)[4:6]
R_Mat = np.array(p.getMatrixFromQuaternion(frame_posture)).reshape(3,3)
x_axis = R_Mat[:,0]
x_end_p = (np.array(frame_start_postition) + np.array(x_axis*5)).tolist()
x_line_id = p.addUserDebugLine(frame_start_postition,x_end_p,[1,0,0])

# y 轴
y_axis = R_Mat[:,1]
y_end_p = (np.array(frame_start_postition) + np.array(y_axis*5)).tolist()
y_line_id = p.addUserDebugLine(frame_start_postition,y_end_p,[0,1,0])


# z轴
z_axis = R_Mat[:,2]
z_end_p = (np.array(frame_start_postition) + np.array(z_axis*5)).tolist()
z_line_id = p.addUserDebugLine(frame_start_postition,z_end_p,[0,0,1])

当欧拉角设置为【0，0，0】时，机械臂摆出如下姿势：

当欧拉角为[0,np.pi/2,0] 时，如下（绕7

轴正向旋转90度，右手定则的正方向）：

当欧拉角为[0, np.pi/2, np.pi/2]时，如下：

可以看到，第三张图相对于第二张图，围绕世界坐标系的z轴正向旋转了90度。

这一点非常重要！！也就是说在pybullet中，欧拉角为静态欧拉角，且与变换顺序无关！

因此，在下一步对末端做坐标旋转变换时，一定注意是相对于世界坐标系的轴向进行的旋转。这种方式更容易思考和在脑海中构建变换图形，不必考虑随体的情形。

本文通过对添加辅助线的方式摸清了坐标轴的定义以及在pybullet中欧拉角的约定方式。