两个程序的区别不是 get_gravity_orientation() 不一样，而是传进去的 quat 已经不一样

input :
d.qpos[3:7]
so:
def get_gravity_orientation(quaternion):
qw = quaternion[0]
qx = quaternion[1]
qy = quaternion[2]
qz = quaternion[3]

gravity_orientation = np.zeros(3)

gravity_orientation[0] = 2 * (-qz * qx + qw * qy)
gravity_orientation[1] = -2 * (qz * qy + qw * qx)
gravity_orientation[2] = 1 - 2 * (qw * qw + qz * qz)

return gravity_orientation

input :
quat = data.sensor(‘orientation’).data[[1, 2, 3, 0]]
so:
def get_gravity_orientation(quaternion):
qw = quaternion[3]
qx = quaternion[0]
qy = quaternion[1]
qz = quaternion[2]

gravity_orientation = np.zeros(3)

gravity_orientation[0] = 2 * (-qz * qx + qw * qy)