def get_ori(self):
ori = [0, 1, 0, 0]
rot = self.model.data.qpos[self.__class__.ORI_IND:
self.__class__.ORI_IND +
4] # take the quaternion
ori = q_mult(q_mult(rot, ori),
q_inv(rot))[1:3] # project onto x-y plane
ori = math.atan2(ori[1], ori[0])
return ori
def get_ori(self):
ori = [0, 1, 0, 0]
rot = self.wrapped_env.model.data.qpos[self.__class__.ORI_IND:self.__class__.ORI_IND+4] # take the quaternion
ori = q_mult(q_mult(rot,ori),q_inv(rot))[1:3] # project onto x-y plane
ori = math.atan2(ori[1],ori[0])
return ori
