def _get_obs(self): quat = self.sim.data.qpos.flat[3:7] #print('quat = ', quat) quat_tranfor = quaternion_multiply(quat, quaternion_inverse(self.quat_init)) angle = euler_from_quaternion(quat_tranfor, 'rxyz') # radian # angles = [] # for i in range(3): # angles.append(angle[i] / pi * 180.0) # print('angle', angles) # angle # # if angle[2]>= 0 and angle[2] < 270: # theta2 = angle[2]/ pi * 180.0+90 # elif angle[2]>= 270: # theta2 = 360-angle[2]/ pi * 180.0 # else: # assert print('angle error! ') # # print('theta = ', theta2) #print('deta = ', angle[2] -self.goal_theta) return np.concatenate([ self.get_body_com("torso").flat, #self.sim.data.qpos.flat[:3], # 3:7 表示角度 #self.sim.data.qpos.flat[:7], # 3:7 表示角度 np.array(angle), np.array( [ angle[2] -self.goal_theta]) # self.goal_theta ])
def get_current_obs(self): quat = self.model.data.qpos.flat[3:7] # print('quat = ', quat) quat_tranfor = quaternion_multiply(quat, quaternion_inverse(self.quat_init)) angle = euler_from_quaternion(quat_tranfor, 'rxyz') #print(self.goal_theta) return np.concatenate([ #self.get_body_com("torso").flat, # self.sim.data.qpos.flat[:3], # 3:7 表示角度 self.model.data.qpos.flat[:7], # 3:7 表示角度 np.array(angle), np.array([angle[2] - self.goal_theta]) ]).reshape(-1)
def _get_obs(self): quat = self.sim.data.qpos.flat[3:7] #print('quat = ', quat) quat_tranfor = quaternion_multiply(quat, quaternion_inverse(self.quat_init)) angle = euler_from_quaternion(quat_tranfor, 'rxyz') # radian # angles = [] # for i in range(3): # angles.append(angle[i] / pi * 180.0) # print(angles) # angle return np.concatenate([ self.get_body_com("torso").flat, #self.sim.data.qpos.flat[:3], # 3:7 表示角度 #self.sim.data.qpos.flat[:7], # 3:7 表示角度 np.array(angle), np.array([angle[2] - self.goal_theta]) # self.goal_theta ])
def _get_obs(self): quat = self.sim.data.qpos.flat[3:7] #print('quat = ', quat) quat_tranfor = quaternion_multiply(quat, quaternion_inverse(self.quat_init)) angle = euler_from_quaternion(quat_tranfor, 'rxyz') # radian object_x = self.get_body_com("torso")[1] object_y = self.get_body_com("torso")[0] target_y = traj[self.cur_line + 1][1] target_x = traj[self.cur_line + 1][0] self.goal_theta = atan((target_y - object_y) / (target_x - object_x)) #print(self.goal_theta/3.1415*180.0) return np.concatenate([ self.get_body_com("torso").flat, self.sim.data.qpos.flat[:7], # 3:7 表示角度 np.array(angle), np.array([angle[2] - self.goal_theta]) ])