def _get_obs(self): robot_qpos, robot_qvel = robot_get_obs(self.sim) object_qvel = self.sim.data.get_joint_qvel('object:joint') achieved_goal = self._get_achieved_goal().ravel( ) # this contains the object position + rotation if self.include_full_object: # dimension: 24:24:6:7 observation = np.concatenate( [robot_qpos, robot_qvel, object_qvel, achieved_goal]) elif self.include_object_rot: observation = np.concatenate( [robot_qpos, robot_qvel, object_qvel, achieved_goal[-4:]]) elif self.include_object_loc: observation = np.concatenate( [robot_qpos, robot_qvel, object_qvel, achieved_goal[:-4]]) else: observation = np.concatenate([robot_qpos, robot_qvel, object_qvel]) if self.include_contact: sim = self.sim num_of_contacts = sim.data.ncon total_num_of_geoms = sim.model.ngeom contacts = [sim.data.contact[i] for i in range(num_of_contacts)] geom1s = [contact.geom1 for contact in contacts] geom2s = [contact.geom2 for contact in contacts] geom1_names = [sim.model.geom_id2name(geom1) for geom1 in geom1s] geom2_names = [sim.model.geom_id2name(geom2) for geom2 in geom2s] geom2_bodys = [sim.model.geom_bodyid[geom2] for geom2 in geom2s] positions = [contact.pos for contact in contacts] forces = [ sim.data.cfrc_ext[geom2_body] for geom2_body in geom2_bodys ] processed, valid_contact_num = process_data( total_num_of_geoms, num_of_contacts, geom1s, geom2s, geom1_names, geom2_names, positions, forces, fixed_num_of_contact=self.fixed_num_of_contact, object_indices=self.object_indices, include_geoms=self.include_geoms, include_force=self.include_force, include_position=self.include_position) self.contact_num = valid_contact_num if self.permute: np.random.shuffle(processed) processed = processed.reshape((-1)) observation = np.concatenate((processed, observation), -1) return { 'observation': observation.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.ravel().copy(), }
def _get_obs(self): robot_qpos, robot_qvel = robot_get_obs(self.sim) object_qvel = self.sim.data.get_joint_qvel('object:joint') achieved_goal = self._get_achieved_goal().ravel( ) # this contains the object position + rotation observation = np.concatenate( [robot_qpos, robot_qvel, object_qvel, achieved_goal]) # # dimension: 24:24:6:7 # sim = self.sim # ngeom = sim.model.ngeom # for idx in range(ngeom): # print(idx, sim.model.geom_id2name(idx)) if self.obs_type == 'object': observation = observation[48:] elif self.obs_type == 'original': pass else: sim = self.sim num_of_contacts = sim.data.ncon total_num_of_geoms = sim.model.ngeom contacts = [sim.data.contact[i] for i in range(num_of_contacts)] geom1s = [contact.geom1 for contact in contacts] geom2s = [contact.geom2 for contact in contacts] geom1_names = [sim.model.geom_id2name(geom1) for geom1 in geom1s] geom2_names = [sim.model.geom_id2name(geom2) for geom2 in geom2s] geom2_bodys = [sim.model.geom_bodyid[geom2] for geom2 in geom2s] positions = [contact.pos for contact in contacts] forces = [ sim.data.cfrc_ext[geom2_body] for geom2_body in geom2_bodys ] processed, valid_contact_num = process_data( total_num_of_geoms, num_of_contacts, geom1s, geom2s, geom1_names, geom2_names, positions, forces, fixed_num_of_contact=self.fixed_num_of_contact, object_indices=self.object_indices, hand_geoms=self.hand_geoms) self.contact_num = valid_contact_num processed = processed.reshape((-1)) self.num_of_contacts = valid_contact_num if self.obs_type == 'contact': observation = np.concatenate( (processed, object_qvel, achieved_goal), -1) elif self.obs_type == 'full_contact': observation = np.concatenate((processed, observation), -1) elif self.obs_type == 'joint_contact': observation = np.concatenate( (processed, robot_qpos, robot_qvel), -1) return { 'observation': observation.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.ravel().copy(), }
def _get_obs(self): # positions grip_pos = self.sim.data.get_site_xpos('robot0:grip') dt = self.sim.nsubsteps * self.sim.model.opt.timestep grip_velp = self.sim.data.get_site_xvelp('robot0:grip') * dt robot_qpos, robot_qvel = utils.robot_get_obs(self.sim) if self.has_object: object_pos = self.sim.data.get_site_xpos('object0') # rotations object_rot = rotations.mat2euler( self.sim.data.get_site_xmat('object0')) # velocities object_velp = self.sim.data.get_site_xvelp('object0') * dt object_velr = self.sim.data.get_site_xvelr('object0') * dt # gripper state object_rel_pos = object_pos - grip_pos object_velp -= grip_velp else: object_pos = object_rot = object_velp = object_velr = object_rel_pos = np.zeros( 0) gripper_state = robot_qpos[-2:] gripper_vel = robot_qvel[ -2:] * dt # change to a scalar if the gripper is made symmetric if not self.has_object: achieved_goal = grip_pos.copy() else: achieved_goal = np.squeeze(object_pos.copy()) obs = np.concatenate([ grip_pos, object_pos.ravel(), object_rel_pos.ravel(), gripper_state, object_rot.ravel(), object_velp.ravel(), object_velr.ravel(), grip_velp, gripper_vel, ]) st() return { 'observation': obs.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }
def _get_obs(self): robot_qpos, robot_qvel = robot_get_obs(self.sim) achieved_goal = self._get_achieved_goal().ravel() observation = np.concatenate([robot_qpos, robot_qvel, achieved_goal]) if self.obs_type == 'object': observation = achieved_goal elif self.obs_type == 'contact' or self.obs_type == 'full_contact': sim = self.sim num_of_contacts = sim.data.ncon total_num_of_geoms = sim.model.ngeom contacts = [sim.data.contact[i] for i in range(num_of_contacts)] geom1s = [contact.geom1 for contact in contacts] geom2s = [contact.geom2 for contact in contacts] geom1_names = [sim.model.geom_id2name(geom1) for geom1 in geom1s] geom2_names = [sim.model.geom_id2name(geom2) for geom2 in geom2s] geom2_bodys = [sim.model.geom_bodyid[geom2] for geom2 in geom2s] positions = [contact.pos for contact in contacts] forces = [ sim.data.cfrc_ext[geom2_body] for geom2_body in geom2_bodys ] processed = utils.process_data( total_num_of_geoms, num_of_contacts, geom1s, geom2s, geom1_names, geom2_names, positions, forces, fixed_num_of_contact=self.fixed_num_of_contact) processed = processed.reshape((-1)) observation = np.concatenate((processed, achieved_goal), -1) return { 'observation': observation.copy(), 'achieved_goal': achieved_goal.copy(), 'desired_goal': self.goal.copy(), }