def ss(self, state_dict, add_noise=False):
if add_noise:
eul = quat2euler(state_dict['desired_orien'])
eul[0] += self.np_random.uniform(low=-self._sigma,
high=self._sigma)
eul[1] += self.np_random.uniform(low=-self._sigma,
high=self._sigma)
state_dict['desired_orien'] = euler2quat(eul)
return super().ss(state_dict)
def reset_model(self):
qp = self.init_qpos.copy()
qv = self.init_qvel.copy()
self.set_state(qp, qv)
desired_orien = np.zeros(3)
desired_orien[0] = self.np_random.uniform(low=-1, high=1)
desired_orien[1] = self.np_random.uniform(low=-1, high=1)
self.model.body_quat[self.target_obj_bid] = euler2quat(desired_orien)
self.sim.forward()
return self.get_obs()