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])
])
